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Preface 


As Global Navigation Satellite Systems (GNSS) such as GPS have grown 
more pervasive, the use of GNSS to automatically control ground vehicles 
has drawn increasing interest. From autonomously driven vehicles such as 
those demonstrated in the DARPA “grand challenges” to automatically 
steered farm tractors, automated mining equipment, and military unmanned 
ground vehicles (UGVs), practical and potential applications of GNSS to 
ground vehicles abound. This text provides an introduction to the concepts 
necessary to understand and contribute to this field. 

It has been said that navigation is “knowing where you are,” guidance 
is “knowing where you re going,” and control is “knowing how to get there.” 
For example, suppose you are sitting at home one hot afternoon and you 
decide to ride your bicycle to the store to get an ice cream cone. 

First, consider navigation. You know that you're starting at home. Fur- 
thermore, you know the names of all the streets all the way to the ice cream 
store and the names of all the nearby streets as well. At any point during the 
trip, you can look up at a street sign and know exactly where you are. You will 
have no trouble with navigation. 

Second, consider guidance. You know where the store is, and you can 
think of many routes to get from your house to the store. Some routes would 
keep you on the paved streets, but it might be faster to take a shortcut across 
the park. It would be even faster to take a shortcut across the river, but the 
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water is too deep; you can only cross at the bridge. You choose a feasible 
route in your mind which meets all the constraints you know of. If you have 
to leave your original route along the way, due to a parade or a street fair, 
you can choose a new route and still reach the store. Guidance will not be a 
problem. 

Finally, consider control. You wheel your bike out the gate, remember- 
ing when you learned to ride it. At first, it wasn’t easy to keep your balance; 
you had to learn to steer in the direction you were falling as soon as you 
started to fall. You also had to learn to steer briefly away from any turn you 
wanted to make, to change your balance so that you could lean into the turn. 
But you understand those algorithms now, and you can implement them fast 
enough to get where you want to go without falling over. Your control of the 
bicycle is adequate. You set off for the store. 

Perhaps this example seems overly complicated. We humans perform 
most of these tasks unconsciously, most of the time. If we want to program 
automated systems to perform them, however, we must first understand 
them in detail. The purpose of this book is to develop an understanding of 
the navigation and control tasks for the special case of ground vehicles. 

The result of a navigation algorithm is the same for every vehicle; your 
position is your position, whether you're on a bike, a bus, or a boat. However, 
each vehicle can have a different set of navigation sensors, and its navigation 
algorithm must be able to use the data from those sensors to compute the 
best possible position. Chapters 1, 2, and 7 of this book describe navigation 
sensors applicable to ground vehicles. Chapter 4 describes navigation algo- 
rithms that combine the data from these sensors to provide the best available 
estimate of position and velocity. 

Control algorithms are different for every vehicle; buses, boats, and bi- 
cycles are steered in very different ways. Chapter 3 describes mathematical 
models for various categories of vehicles, with a particular emphasis on com- 
mon four-wheeled vehicles. These models are used in Chapters 5 and 6 to de- 
velop vehicle control and estimation algorithms for tasks such as autonomous 
steering and electronic stability control (ESC). 

The problem of guidance is not so easy to generalize. Each type of ve- 
hicle has different constraints on the routes it can take. In addition, a given 
vehicle’s mission imposes other goals and constraints not shared even by simi- 
lar vehicles on different missions. Guidance is therefore largely beyond the 
scope of this text. 

The authors expect this book to be a useful introduction, for a gradu- 
ate engineer or perhaps an advanced undergraduate, to the problems of 
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navigating and controlling ground vehicles automatically. Because Global 
Navigation Satellite Systems (GNSS) are now widely available, Chapter 1 
discusses concepts and navigation algorithms related to GNSS and to 
ground-based navigation transmitters known as “pseudolites.” Chapter 1 
also introduces inertial measurement instruments, which sense acceleration 
and rotation directly; compasses, which sense heading relative to the Earth’s 
magnetic field; and odometers, which measure distance by counting the ro- 
tations of a wheel. Higher-level navigation algorithms, which combine data 
from these sensors, are presented in Chapter 4. 

Chapter 2 describes the use of “machine vision” algorithms to detect 
a vehicle’s position relative to road features such as lane markers, as seen 
through vehicle-mounted cameras and laser scanners (Lidar). These are espe- 
cially useful for lateral navigation of a vehicle on a road for Lane Departure 
Warning (LDW) or lane-keeping assistance. These navigation algorithms 
combine locally generated measurements from a camera or Lidar with global 
measurements from GPS and a map database to form a robust measurement 
of position in the lane. 

Chapter 3 introduces models for vehicles, tires, suspensions, and trail- 
ers. These models describe the behavior of highway vehicles such as passenger 
cars and SUVs as well as off-road vehicles such as farm tractors and unmanned 
ground vehicles (UGVs). This chapter describes the lateral and longitudinal 
dynamics that arise from these models and compares model data with mea- 
sured data for a particular vehicle. 

Chapter 4 describes methods for creating and updating an estimate of 
the navigation state using various combinations of the sensor measurements 
discussed in Chapter 1. These methods are based on the concept of the Kal- 
man filter (a tutorial review of filtering and estimation techniques is provided 
as the Appendix to this book). Again, results from a simulation model are 
compared to data measured on a particular vehicle. 

Chapter 5 describes methods for estimating parameters specific to four- 
wheeled vehicles, which are critical for modeling and control of vehicles. 
These estimated parameters can then be incorporated into a mathematical 
model of that specific vehicle for online vehicle modeling and vehicle control. 
Chapter 6 develops and analyzes control algorithms tuned to the models de- 
veloped in the previous chapters. 

Finally, Chapter 7 gives a detailed description of “pseudolites,” 
ground-based transmitters of signals similar to GNSS signals, which are 
useful for navigating ground vehicles in restricted areas such as open-pit 
mines. 
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Ground vehicles can navigate using signals from external navigation systems 
such as the Global Positioning System (GPS) (Figure 1.1), or by using sig- 
nals from internal devices such as a compass, an odometer, a gyroscope, an 
accelerometer, or a full-blown inertial navigation system (INS). In practice, 
the most reliable and accurate navigation is obtained by combining data from 
all available sources, including static databases such as a digitized map. This 
chapter will discuss many of these sources of navigation data, and methods 
for combining them. 


1.1. Global Navigation Satellite System (GNSS) 


There are at least two Global Navigation Satellite Systems (GNSSs) currently 
in existence, and several more are proposed. These systems are all similar in 
concept, differing in small details of signal frequencies, signal structure, and 
orbit design. The concepts presented in this chapter should apply to any 
GNSS system, regardless of those differences. Where specific details must be 
cited, the United States’ GPS will be used as an example. GPS is the most 
thoroughly studied GNSS, and the most useful at present. 
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1.1.1 Description of a Typical GNSS 


A GNSS receiver navigates by precisely measuring the range between its an- 
tenna and a set of transmitter antennas at precisely known locations, and 
then performing a triangulation algorithm to determine its position. This is 
not quite as easy as it sounds, because the transmitters are aboard satellites 
moving rapidly through space, and the measurements must be made with 
nanosecond precision. 

The space segment of a GNSS consists of a group, or constellation, of 
satellites in orbits that circle the Earth about twice per day. In order to pro- 
vide adequate signal coverage to the whole earth, a constellation typically 
consists of 20 to 30 satellites in three to six different orbital planes. Some 
systems also include satellites in geosynchronous orbits. 

The satellites broadcast microwave signals toward the Earth. Each satel- 
lite is far enough from Earth that its signal covers most of a hemisphere. Each 
signal consists of a carrier wave at a frequency near 1.6 GHz, modulated by 
a stream of digital bits at a rate of about 1 million bits per second (1 Mbps). 
The digital bits are generated in a way that is actually systematic but which 
appears random, and are called a pseudorandom noise code or PRN code. Each 
satellite has its own specific PRN code. The PRN code is itself modulated by 
digital navigation data at a slow rate (typically 50 bits per second). 


Figure 1.1 GPS satellite constellation, approximately to scale. 
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The frequency of each satellite’s signal and the bit rate of its PRN code 
are controlled by an extremely precise clock (an atomic clock) on board the 
satellite. The uncompensated drift rate of each satellite’s clock is typically a 
few nanoseconds per day. The satellite signal is designed so that a receiver 
which “hears” the signal can read the exact time of the satellite’s clock at the 
instant the signal was transmitted, with an error of a few nanoseconds. 

Each GNSS has a master control center, which constantly listens to the 
satellite signals through receivers in several different locations. It uses this 
information to compute the exact orbits and clock drift corrections for all 
the satellites, and transmits this information to each satellite in turn. This 
information is then broadcast by the satellite as part of the navigation data 
message. Each user receiver can interpret the navigation data to determine 
the precise time (according to the whole GNSS system, not just a particular 
satellite clock) that a signal was transmitted from a satellite, and the precise 
position of the satellite (within a meter or so) when it was transmitted. 

At a particular instant (chosen according to its own internal clock), the 
receiver takes a snapshot of the clock readings of each satellite it can “hear.” 
It then subtracts the reading of its own internal clock from the readings of 
the satellite clocks. The difference is the time taken by each signal to travel 
from its satellite to the receiver. Assuming that the signal traveled at the speed 
of light from the satellite directly to the receiver, the receiver can divide that 
time by the speed of light to compute the distance, or pseudorange, between 
that satellite and the receiver. (This distance is called “pseudorange” rather 
than simply “range” because it is in error by the amount that the receiver’s own 
internal clock is in error. The next section shows how this error is corrected.) 

The receiver then computes the position of each satellite at the time its 
signal was transmitted, using information contained in the navigation data 
message. Finally, knowing the distances (at the sampling instant) between its 
own position and several known points (the satellite positions), the receiver 
computes its own position, as shown in the next section. 

For the remainder of this text, the GNSS receiver will be treated as a 
black box that provides pseudorange measurements and satellite positions. 
The details of the technology by which this is done are fascinating but are 
outside the scope of this book. The interested reader is referred to [1, 2]. 


1.1.2 Simple (Pseudorange) GNSS Navigation 


Figure 1.2 illustrates the geometry of a pseudorange measurement. The range 
vector vector r;, is the difference between the position of the user receiver (r,,) 
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Figure 1.2 Geometry of a pseudorange measurement. 


and the position of satellite 7 (r,). The pseudorange measurement includes the 
delay due to the length of this range vector along with various errors. Each 
pseudorange can be written as 


Pi =| ty - 1; | +0 %, +E; (1.1) 
where 


p; is the pseudorange measurement for satellite 7; 
r, is the position of the user receiver; 

r, is the position of satellite 7; 

b,, is the receiver’s clock bias; 

c is the speed of light; 


€, is the sum of measurement errors associated with satellite 2. 


To find its position, the receiver applies the principle of triangulation. 
Three known points (the satellite locations) define a plane, and the ranges to 
these points uniquely define two possible receiver locations, one above and 
one below the plane. One of these is generally far out in space and can be 
eliminated by inspection, leaving the other as the true receiver position. 

In practice, the receiver must solve for its internal clock bias as well as 
for the three dimensions of its position, a total of four unknowns. The re- 
ceiver needs pseudorange measurements from four different satellites in order 
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to solve for those four unknowns. The solution process typically updates an 
initial position estimate f,, using new measurement data. If no better initial 
position estimate is available, one can be obtained by averaging the subsatel- 
lite points for all satellites currently being tracked. 

Given this initial position estimate and the known satellite positions, 
the difference p; between the estimated and measured pseudoranges can 
then be written as 


U 
(y+De; (1.2) 
u 


where 


1, is the unit vector from satellite 7 to the receiver’s estimated position; 
r,=f,,—1,,is the difference between the receiver’s estimated and actual 
positions; 


Db, =, - 6, is the difference between the receiver’s estimated and ac- 
tual clock errors. 


Four (or more) of these pseudorange equations can be stacked to form 
the matrix equation 


Dp =GDx +De (1.3) 
where 
Z ‘ AT r Z x 
éDp; U al ' éDe; t 
U we 4 v4 * L 
aLO2 al 1U é Dr, U ADE 2 7 
Dp? su G2g’ 4 Dx OG and Deeg. 't 
co OM A: ‘ eeu eae: 
&D f eer i De, 
Pru , 1¢ nl 


A correction £, to the initial position estimate £, can be obtained from 
this matrix equation by the method of least squares: 


Di =(G’ G)"'G" Dp (1.4) 
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The result ¥ contains the position correction f,, as well as a correc- 
tion 6, to the clock bias estimate. The updated, accurate position £7 is 
then computed as 


ty” =t, - Db, (1.5) 


This process is repeated until the correction & is negligibly small. 

When more than four pseudoranges are used in the navigation algo- 
rithm, a least-squares residual pp” can be calculated from the final position 
and the measured pseudoranges. When available, this residual is often used as 
a measure of the quality or accuracy of the new position estimate, in a process 
known as receiver autonomous integrity monitoring (RAIM) [2, Chapter 5]. 

Another quality measure, which is always available, is provided by the 
position covariance matrix A (G’G)"'. The square root of the trace of A 
is known as the geometric dilution of precision (GDOP). GDOP describes 
the accuracy degradation of the position solution due solely to the relative 
positions of the satellites. Various components of GDOP describe the deg- 
radation in particular dimensions: horizontal DOP (HDOP), vertical DOP 
(VDOP), position DOP (PDOP), and time DOP (TDOP). The GDOP 
concept assumes that the errors in the individual pseudorange measurements 
are uncorrelated and have the same statistics. This concept is explained fur- 
ther in [1, Chapter 11]. 

This abbreviated discussion of the unassisted GNSS pseudorange navi- 
gation algorithm is provided as background for this chapter’s discussion of 
enhancements to GNSS pseudorange navigation. For a more detailed expla- 
nation of this algorithm, please refer to [1, Chapter 9], which this presenta- 
tion closely follows. 


1.1.3 Differential GNSS Navigation 


The GNSS signals measured by the user’s receiver contain a number of errors 
or deviations from the mathematical ideal. The actual position of the GNSS 
satellite is only known to within a meter or two, and the timing of its clock 
may be off by a few nanoseconds. The radio signal from the satellite is de- 
layed as it travels through the ionosphere and troposphere. The receiver can 
be fooled by signal reflections from nearby objects, known as multipath. All 
these errors except multipath are spatially correlated; that is, the sum of these 
errors will be similar for all receivers within a given area. 
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1.1.3.1. Local Area Differential GNSS Navigation 


The local area differential GPS (LADGPS) technique reduces spatially cor- 
related errors in the GNSS satellite signals to negligible levels. A LADGPS 
reference receiver, installed at a well-known location, computes an assumed 
pseudorange for each satellite signal it detects. It then measures the pseudo- 
range for that satellite signal and subtracts the assumed pseudorange, forming 
a differential correction. The LADGPS reference station transmits these cor- 
rections as digital data to nearby user receivers. 

Each user receiver adds this correction to the pseudorange it measures 
for the same satellite before performing the navigation algorithm described in 
the previous section. Errors common to both receivers, such as satellite clock 
errors, are entirely removed by this procedure. Other errors, such as iono- 
sphere and troposphere delays and satellite ephemeris errors, are removed 
to the extent that they are spatially correlated. Uncorrelated errors, such as 
multipath and receiver noise, add directly to the user’s navigation error, but a 
high-quality DGPS reference receiver will minimize them. User receivers are 
often within line-of-sight to the reference receiver, so LADGPS corrections 
are often broadcast by the reference receivers on short-range digital radio 
links. 

DGPS concepts are further described in [2, Chapter 1]. 


1.1.3.2 Network Differential GNSS Navigation 


Local area DGPS, as its name implies, requires a reference receiver within the 
local area of the user receiver. If the area of interest is large, many LADGPS 
reference receivers are required. Attempts to reduce the number of reference 
receivers required led to the concept of network differential GNSS navigation 
(NDGPS). 

In network DPGS, a small number of widely spaced reference receivers 
are connected together with high-speed datalinks. By comparing their simul- 
taneous measurements of a given satellite, they can estimate which portions of 
their measured signal errors are spatially correlated, and which are not. When 
a user receiver contacts the network and asks for differential corrections, the 
network projects the spatially correlated errors to the user’s position, adds 
the uncorrelated errors, and transmits a set of differential correction data 
specific to that user’s location. Because the area covered by the system is large, 
a user receiver is rarely within line-of-sight to a reference receiver. Network 
DPGS corrections are often distributed via data networks such as the cellular 
telephone system. Networks are often operated by state governments in the 
United States, by small countries elsewhere in the world, and by commercial 
organizations. 
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1.1.3.3 Wide Area Differential GNSS Navigation 


The network GPS concept can be extended to cover an entire continent. 
This is known as wide area differential GNSS navigation (WADGPS). With 
many widely spaced reference receivers, the network can track a GNSS satel- 
lite over a large fraction of its orbit. By doing so, the network can compute 
accurate ephemeris and clock information for each satellite, regardless of the 
navigation data broadcast by the satellite. By tracking many satellites at once 
through many widely spaced receivers, the network can accurately compute 
the delays due to the ionosphere (and in some cases the troposphere) over 
most of the continent. The network can then transmit a set of processed data 
that any receiver on the continent can use to compute its own differential cor- 
rections. This data can even be distributed over a satellite broadcast channel 
to all user receivers on the continent. This is known as a space-based augmen- 
tation system (SBAS). An international standard for SBAS has been defined, 
and at least three compatible SBAS systems currently exist. 

The United States operates an SBAS called the Wide Area Augmentation 
System (WAAS), which has been operational for several years. The European 
Union (EU) operates another SBAS, called European Geostationary Navigation 
Overlay Service (EGNOS), which is in the process of becoming operational. 
Japan operates yet another SBAS, the MTSAT Satellite-based Augmentation 
System (MSAS), which became operational in 2007. All these systems broadcast 
their corrections through geostationary satellites, using a signal format that is 
compatible with the GPS navigation signal format. This means that a user re- 
ceiver needs no additional hardware, only some software, to take advantage of the 
SBAS corrections. Because the incremental cost of software is very low, and the 
signals are free to use, most new civilian receivers have SBAS capability built in. 
Although the accuracy varies with time, most receivers using an SBAS signal can 
expect to navigate within 2 meters of their actual position, most of the time. 

There are also commercial providers of WADGPS services. The com- 
metcial systems transmit differential correction data using higher data rates 
than the government systems use, and claim higher accuracy as a result. These 
services are available by subscription and can cost tens or hundreds of dol- 
lars per month. The best-known of these services are OmniStar, operated by 
Fugro, and StarFire, operated by John Deere. 


1.1.4 Precise (RTK) GNSS Navigation 


As it travels through space at the speed of light, each bit of a GNSS satellite’s 
PRN code is about 300 meters long. Each cycle of the carrier frequency is 
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about 19 cm long. These are the features of the GNSS signal that receivers 
can measure. A good receiver can measure either feature with a precision of 
a fraction of 1 percent. The precision in range is about 0.5 meter for the PRN 
code and about 1 mm for the carrier phase. This improvement in measure- 
ment precision can permit a corresponding improvement in position accu- 
racy—giving real-time positions with an error of only a few centimeters— 
once the problem of carrier-phase ambiguity is solved. 

The PRN code is designed to be unambiguous; each bit of the code has 
a distinct signature and cannot be confused with its neighbors. Because of 
this, a receiver's PRN code measurement gives the pseudorange directly. This 
is not true for carrier phase measurements. Carrier cycles are not unique; each 
cycle looks just like every other cycle. The receiver can measure the fractional 
phase plus an arbitrary number of whole cycles, but cannot directly determine 
the exact number of whole cycles in the pseudorange. This number, known 
as the integer cycle ambiguity, must be determined by means other than direct 
measurement. Figure 1.3 illustrates these concepts. 

As the fractional carrier phase passes through zero in the positive or 
negative direction, the receiver can increment or decrement an integer coun- 
ter as appropriate. The relative carrier phase measurement consists of the 
instantaneous value of the integer counter plus the fractional phase. This 
measurement is also known as integrated Doppler or carrier beat phase or 
accumulated delta range (ADR). The integer cycle ambiguity is the difference 
between this relative carrier phase measurement and the actual pseudorange 


Line-of-sight vector 
(user to satellite) 


LS LS 


Reference receiver User receiver 


Figure 1.3. Carrier-phase measurements and integer ambiguity. 
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at any given instant. This integer ambiguity remains a constant for each sig- 
nal as long as the receiver maintains continuous tracking of that signal. 

Although it is theoretically possible to navigate using carrier-phase 
pseudoranges to the various satellites, carrier-phase navigation in practice is 
always done differentially. A reference station (or network) computes relative 
carrier phase measurements for each satellite in view at a fixed location, and 
transmits those measurements to the user receiver. The user receiver subtracts 
the reference measurements from its own similar measurements, forming a 
set of differential carrier-phase pseudorange measurements of the form 


dpi =(r, = ty) a; +N; +deé; (1.6) 
where 


dp; is the differential carrier phase measurement for satellite 7; 
r, is the position of the user receiver; 

r,is the position of the reference receiver; 

1, is the unit vector from the user to satellite 2; 

N, is the integer ambiguity associated with satellite 7; 


dé; is the sum of differential measurement errors associated with satel- 
lite 2. 


The integer ambiguities NV; cannot be measured from the instantaneous 
GNSS signals but must be determined by other means. Methods for accom- 
plishing this quickly and reliably remain an active area of research, but fil- 
tering sets of satellite measurements over time can generally determine the 
integer ambiguities with high confidence. Once the ambiguities are known, 
the position solution can be found using the algorithms of Section 1.1.2. 
This process is frequently called real-time kinematic (RTK) navigation be- 
cause it was first developed for kinematic surveying applications. When done 
correctly, it can provide real-time GNSS positions with an accuracy of 1 to 
2 cm. 

In practice, most LADGPS and network DPGS systems are currently 
being used as reference stations for RTK navigation. WADGPS systems use 
reference stations that are too widely spaced to provide accurate carrier-phase 
reference data. 

GPS carrier measurements can also be used to provide accurate three- 
dimensional velocity measurements. For ground vehicles, the velocity mea- 
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surements give the direction of travel, also known as course over ground or 
simply vehicle course. A GPS receiver with multiple antennas can also mea- 
sure the vehicle attitude (heading, pitch, and roll) in two or three dimensions 
[3, 4]. 

A two-antenna GPS receiver provides noisy, but unbiased, measure- 
ments of vehicle course Vand heading Yas follows: 


Vi?) =v Hy (1,7) 


yO =y ny (1.8) 


The sampled noise on these measurements for a typical vehicle installa- 
tion can be modeled as: 


> 2.05 m/s o 
Ov = = 
ve®@ (1.9) 


Pe _ 47 radians o 
v = 
& 180 4) (1.10) 


Note that the accuracy of the course estimate will improve with vehicle 


speed [5]. 


1.1.5 Current and Future GNSS Constellations 


1.1.5.1 GPS 


The best-known GNSS constellation is the United States’ GPS system. This 
system became operational some 15 years ago, after almost two decades of 
testing, and has continued to evolve and improve ever since. It is built and 
operated by the U.S. Air Force. It was originally designed as a military sys- 
tem, but the main navigation signal was made available to civilians from the 
beginning. The published policy of the U.S. government is to keep the civil 
signals available and free to use worldwide. The specified accuracy of the civil 
navigation signals is 9 meters horizontal (95%) and 15 meters vertical (95%) 
under certain specified assumptions [6]. 

At present, the number of operational GPS satellites is generally in the 
high 20s, giving worldwide navigation availability with generally good PDOP. 
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The official technical documents describing the GPS system are known as In- 
terface Standards (IS). The primary document is IS-GPS-200 [7]. 


1.1.5.2 GLONASS 


The other existing GNSS constellation at present is the Russian GLONASS 
system (“GLONASS” is a Russian-language acronym meaning “global navi- 
gation satellite system”). It has been in development for almost as long as 
GPS, but has achieved somewhat less operational success. Many GLONASS 
satellites have been launched, but they tend to fail more quickly than the 
GPS satellites. As a result, a full constellation of GLONASS satellites has 
rarely been seen. An improved satellite design has recently entered service, 
and many more such satellites are scheduled to be launched soon, so that a 
full GLONASS constellation may be available in the next year or two. The 
official technical document describing the GLONASS system is [8]. 


1.1.5.3 Galileo 


The European Union plans to build a GNSS to be known as “Galileo.” Both 
the United States and the EU have worked hard to ensure that Galileo will 
be compatible with GPS. While Galileo’s technical details have been fairly 
clear for a while, the program was stalled for years by confusion regarding 
business models and sources of funding. This has now been resolved, and 
two experimental satellites have been launched. The first validation spacecraft 
are expected to be launched in late 2010. The official technical document 
describing the Galileo system is [9]. 


1.1.5.4 Compass 


The People’s Republic of China has announced plans to build a GNSS of 
their own, known as “Beidou” in Mandarin or “Compass” in English. No of- 
ficial technical documents describing the Compass system have been released 
to date. What little information is available tends to indicate that Compass 
is similar to, and generally compatible with, GPS and Galileo. At least one 
experimental Compass satellite has been launched. 


1.1.5.5 QZSS 


Japan has announced the Quasi-Zenith Satellite System to improve naviga- 
tion within the Japanese islands. Unlike the other constellations, QZSS 
consists of three satellites in geosynchronous (but not geostationary) orbit. 
The satellites are in inclined elliptical orbits, so that they appear to “hang” 
almost directly over the Japanese islands for more than half of each orbit (Fig- 
ure 1.4). With three satellites following one another around the same ground 
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Figure 1.4 Japanese QZSS constellation ground track. 
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track, at least one will be nearly overhead (“quasi-zenith”) at any given time. 


« 


The QZSS broadcast signals are intended to be compatible with GPS signals. 


The official technical document describing the QZSS system is [10]. The first 


QZSS satellite is expected to be launched in 2010. 
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1.2 Pseudolites 


The previous section illustrated that GNSS receivers generally must measure 
pseudoranges from at least four different satellites simultaneously in order to 
navigate effectively. Four equations (the pseudoranges to each satellite) are 
used to solve for four unknowns (X, Y, Z, and time). 

It is possible in some cases to navigate with less than four satellite sig- 
nals. For example, it may be reasonable in a ground vehicle to assume that 
altitude (Z) is constant for short periods, so that navigation can proceed tem- 
porarily with only three signals. In other systems, individual pseudorange 
measurements may be incorporated into a model of vehicle dynamics that is 
also being updated with other data. For some applications, navigation with 
such a model is “good enough.” 

Other applications, however, will require at least four pseudoranges 
at all times. In many locations, foliage or terrain features block the signals 
from most of the satellite constellation. For example, Figure 7.1 in Chapter 7 
shows a diagram of a deep open-pit or open-cast mine, typical of many cop- 
per mines worldwide, in which the walls of the pit block many satellite sig- 
nals. Nevertheless, the mine’s profitability depends on accurate positioning 
of each drill bit and each shovelful of ore. It is for these applications, in these 
locations, that pseudolites are useful. 


1.2.1 Pseudolite Basics 


The term pseudolite is derived from “pseudo-satellite.” Pseudolites are ground- 
based devices that transmit navigation signals similar to those transmitted by a 
GNSS satellite. A pseudolite receiver uses these signals to calculate its position, 
just as conventional GNSS receivers use the GNSS satellite signals. 

Some pseudolite systems are designed so that the receiver normally 
navigates with GNSS signals, and needs only one or two pseudolite signals to 
supplement the satellite signals when satellite visibility is poor. Other pseudo- 
lite systems are designed to operate in a stand-alone manner, entirely separate 
from any GNSS system. Both types have their uses, as explained in the fol- 
lowing section. 


1.2.2 Pseudolite/GNSS Navigation 


Pseudolites can be designed to supplement conventional GNSS navigation. 
Such a pseudolite transmits a signal very similar to a GNSS satellite signal, 
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and a user receiver can use the signals from one or more pseudolites—with 
or without the signals from GNSS satellites—to find its position. The user’s 
navigation algorithm is similar to the satellite-only navigation algorithm de- 
scribed above, but it must be extended to cover certain differences between 
satellites and pseudolites. 

Like the satellites, such a pseudolite contains a very precise clock syn- 
chronized to the GNSS system time. Achieving this synchronization can 
be difficult. The GNSS satellite clocks are monitored and adjusted by the 
GNSS system control center, which generally has an estimate of the amount 
by which each satellite clock is in error at any time. A pseudolite can only 
observe a fraction of the satellites at any time, and unless it is linked the con- 
trol center, it has no knowledge of the instantaneous error of any satellite. 
The control center defines the GNSS system time, but the pseudolite must 
estimate it, so the pseudolite’s clock will generally contain more error than 
the satellites’ clocks. The navigation algorithm must weight the pseudolite 
and satellite pseudorange measurements appropriately to reach an optimum 
position solution. 

A pseudolite transmits its signal, not from orbit, but from a fixed loca- 
tion on the Earth’s surface. The navigation algorithm must be made aware 
of the pseudolite’s location, either through the navigation data within the 
pseudolite’s signal, or through a database or a separate transmission. 

The satellite-only navigation algorithm implicitly assumes that the sat- 
ellites are infinitely far away, so that the line-of-sight vectors to the satellites 
do not change significantly as the algorithm converges on the receiver’s posi- 
tion. This is a reasonable assumption, because the distance from a satellite to 
the user is generally several times the radius of the Earth. This assumption is 
not valid for pseudolites, which may be quite close to the receiver. The navi- 
gation algorithm must recalculate the line-of-sight vectors to the pseudolites 
during each iteration. This makes the navigation problem nonlinear, which 
can cause a simplistic algorithm to converge slowly or to fail to converge 
at all. 

All of these problems have been solved in various pseudolite systems. In 
fact, the GPS concept itself was originally tested using pseudolites placed on 
desert mesas, before any satellites had been launched. 


1.2.3 Differential Pseudolite/GNSS Navigation 


Precise clocks are expensive, and synchronizing them to GNSS system time 
can be difficult. An alternative is to build pseudolites with less expensive, 
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less precise clocks, and to use a separate fixed receiver to provide differen- 
tial corrections. This is an extension of the “differential GNSS” concept de- 
scribed above. The reference receiver takes simultaneous measurements of 
the pseudolite signals and the GNSS satellite signals and broadcasts the set 
of measurements to other “rover” receivers nearby. This set of measurements 
contains the information needed to determine the pseudolites’ clock errors 
relative to GNSS system time at the instant of measurement. The rover re- 
ceivers can use this data to determine their own positions more accurately, 
just as in conventional differential GNSS navigation. 

It can be difficult to find a location for the reference receiver which al- 
lows it to monitor signals from all the pseudolites and all the satellites that 
every rover receiver wants to use for navigation. In such cases, two or more 
reference receivers can be used. If at least one signal (typically from a high- 
elevation satellite) is measured by all reference receivers, that signal can be 
used. as a common time reference to determine the differences between the 
internal clocks of all the reference receivers. Once these differences are known, 
all of their measurements can be projected to a common time and used as if 
there were only one reference receiver taking all the measurements. 


1.2.4 Pseudolite Self-Synchronization 


An extension of this concept is to provide the pseudolites themselves with 
receivers capable of measuring signals from other pseudolites as well as from 
the GNSS satellites. Each individual pseudolite can then serve as a refer- 
ence receiver, transmitting its measurements as data on its own pseudolite 
signal. This simplifies the installation of a pseudolite system, because a sepa- 
rate reference receiver (with its difficult location constraints) is no longer 


needed. 


1.2.5 Stand-Alone Pseudolite Navigation 


Taking the concept above to its logical extreme, a pseudolite system can be 
built that does not depend on GNSS at all. The pseudolites synchronize their 
clocks to each other rather than to a satellite reference. It is also possible for 
a fraction of the pseudolites to determine or refine their own positions dur- 
ing this process. The locations of a few pseudolites must be determined by 
external means, because the synchronization technique can only determine a 
pseudolite’s location relative to other pseudolites in the system, not its abso- 
lute location. 
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1.2.6 Conflicts with GNSS Frequencies 


There are few technological restrictions on the signal format or frequency 
that a pseudolite can transmit, but there are practical and legal restrictions. 
It might seem attractive to design a pseudolite system to transmit a signal 
on the same frequency as a GNSS system, with a similar signal format. This 
would seem to allow a simple GNSS receiver to take advantage of pseudolite 
signals as well, without requiring new radio hardware or much new software. 
Unfortunately, the real world is not so simple. 

All GNSS satellite signals are weak—typically well below the level of 
thermal noise—so that the receiver must integrate its input for a long time to 
detect a usable signal. An incoming signal much stronger than the weak satel- 
lite signals can disrupt this integration process, obliterating the weak signals. 
The strength of a radio signal changes with the inverse square of the distance 
between the transmitter and the receiver. By design, the GNSS satellites are 
all at large and relatively constant distances away from a ground-based re- 
ceiver, so the receiver does not see a satellite’s signal strength vary widely 
while it is in view. Because of this and other clever features of the GNSS sat- 
ellite signals, a user receiver can measure signals from many satellites at once, 
all transmitting on the same frequency. 

A ground-based pseudolite can be designed and adjusted to present a 
similar received signal level to a user receiver a fixed distance away. As the user 
receiver moves, however, the signal level it receives from the pseudolite will 
change. It is not hard to imagine scenarios in which the distance between a 
pseudolite and a receiver can change by a factor of 100 or more, leading to a 
change in signal strength of 40 dB or more. Most nonmilitary GNSS receiv- 
ers will be jammed into uselessness by an incoming signal 40 dB stronger 
than the satellite signals. It is also possible for a receiver to move far enough 
away from a pseudolite that it can no longer measure the pseudolite signal. 
This is less of a problem in practice, because more pseudolites can usually be 
added to fill in dead spots in a coverage area. 

The variation of pseudolite signal strength with distance to a given re- 
ceiver is known as the near/far problem in pseudolite research. It has been 
extensively studied [11], but no general solution exists. The best partial solu- 
tion is to transmit a strong pseudolite signal in short pulses. The receiver can 
integrate and measure the weak satellite signals in between the pulses, and 
the pulses are strong enough (well above the noise level) that the pseudolite 
signal can be measured directly, without integration. Nevertheless, a pulsed 
pseudolite can still jam a GNSS receiver which is not designed to cope with 
pseudolite signals. 
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Recognizing this problem, most governments have set very tight limits 
on the amount of signal power that can be deliberately transmitted on the 
GNSS frequencies. These limits effectively prohibit the use of pseudolites on 
the GNSS frequencies in any outdoor application. In response, recent pseu- 
dolite systems have been designed to transmit their signals in other frequency 
bands, notably the license-free band near 2.45 GHz. 


1.3 Inertial Navigation Systems (INS) 


Since the motion of objects is governed by only a few physical rules, having 
some measure of these quantities allows self-contained navigation to be pos- 
sible. Inertial sensors provide a link between electronics and the motion of 
a body they are monitoring by relating motion to signals. One advantage of 
inertial sensors over ranging systems such as GPS is the independence from 
external systems. Whereas a GPS signal can be blocked, jammed, or spoofed, 
inertial measurement units (IMUs) are immune to these external effects. This 
means that so long as the unit itself is operating correctly, motion informa- 
tion will be available to the user. Also, this information will be accurate to the 
specifications of the sensor itself. This advantage has been used for decades in 
various scenarios such as vehicle, aircraft, and missile navigation. The costs 
of manufacturing inertial sensors has been decreasing rapidly, so that sensors 
such as those shown in Figure 1.5 are now available at prices appropriate for 
vehicle navigation and control systems. Inertial sensors generally fall into two 
categories: accelerometers and gyroscopes. 


1.3.1 Linear Inertial Instruments: Accelerometers 


Accelerometers are used to relate signal levels to sensed specific forces along 
a particular sensor axis. These specific forces can be due to field reactions be- 
tween bodies such as the attraction due to gravity, or inertial forces because of 
a change in motion. These latter accelerations are the values that are usually 
desired since keeping track of motion change allows the navigation system to 
track change in velocity and thus change in position. Therefore, given some 
initial position/velocity, these navigation states can be tracked by integrating 
the accelerations sensed by the accelerometer [12]. 

Most modern accelerometers in use in the vehicle dynamics field are 
microelectromechanical systems (MEMS). These devices operate on a proof- 
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Figure 1.5 Single-axis accelerometer and gyroscope made with MEMS technology (fore- 
ground) and a three-axis IMU built with similar sensors (background). 


mass concept relating to Newton’s second law. A diagram of a simplified 
model for an accelerometer is shown in Figure 1.6. 

This example accelerometer can be modeled as a dynamic system with 
an equation of motion 


mk +bx +kx =bua +ku (1.11) 


This yields a system transfer function relating relative displacement 
d=x uto housing motion, uw, 


D = -m 
U— ms* +bs +k 


(1.12) 
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Figure 1.6 Accelerometer proof-mass model along a sensitive axis. 


Therefore in steady state, the displacement is a measure of the accel- 
eration of the accelerometer housing. Since this is the desired measurement, 
steady state conditions are desired, and therefore the accelerometer dynamics 
are much faster than those expected to be experienced by the housing. In 
this case, the accelerometer can be considered a scaled measurement of the 
housing acceleration, which is a function of the specific forces applied to the 
accelerometer housing. 


1.3.2 Angular Inertial Instruments: Gyroscopes 


Gyroscopes measure the rotation rate about a sensitive axis. Several types of 
gyros exist of varying accuracy, cost, and mechanics. Two typical gyroscope 
types found in vehicle applications include optical and vibratory type gyro- 
scopes. Optical gyros operate by detecting changes in the path traveled by 
light as shown in Figure 1.7. When the gyroscope rotates along the same di- 
rection as the light, the light must travel further along each path segment, re- 
sulting in a longer path. The same is true in the opposite direction [13, 14]. 

Vibrational gyroscopes generate harmonic motion in a structure (typi- 
cally MEMS) at a known high frequency. Considering the relative accelera- 
tion of two points, the relative acceleration can be expressed as 


a =\W\r - 2W. 47 (1.13) 


Ay Py 


(a) (b) (c) 


Figure 1.7 Light path for optical gyroscope to detect rotation with (a) no rotation, (b) 
rotation along the path, and (c) rotation opposite the path. 
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Figure 1.8 Generating attitude from ideal gyroscope measurement. 


Since the relative position is induced by a periodic vibration, the 7, 7, 
and # terms are sinusoidal functions with factors of distance, distance times 
frequency, and distance times frequency squared. At a high vibration rate, the 
terms without frequency can be neglected and the resulting sensed output is 
proportional to _, the angular rate to be detected [13]. 


1.3.3 Ideal Inertial Navigation 


The use of inertial sensors allows for a navigation system to actively monitor 
its motion. In the field of vehicle control, however, the motion is not the only 
effect that needs to be provided to navigation and control subsystems. Often 
it is desired to know position and attitude from these motion measurements. 
Since only derivatives of these quantities are provided by inertial sensors, 
integration must occur to have useful position, velocity, or attitude informa- 
tion. Figure 1.8 shows the steps necessary to generate attitude from a single 
gyroscope output. Similarly, Figure 1.9 shows the steps to calculate velocity 
and position from a single accelerometer output. 

As an example of the use of inertial sensors in navigation solution calcula- 
tion, consider a simplified two-dimensional terrain [15]. A vehicle is equipped 
with longitudinal and lateral accelerometers and a yaw gyroscope positioned 
in the terrain with respect to an origin point as shown in Figure 1.10. 

The position and heading of the vehicle are desired in a north-east co- 
ordinate frame, but the measurements are given in a coordinate frame aligned 
with the vehicle (body frame). In the ideal case, the gyro measurements are 
equivalent to the heading rate. Therefore, given the initial vehicle heading, 


Te 


Figure 1.9 Generating velocity and position from ideal accelerometer measurement. 
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Figure 1.10 Position and heading of a vehicle in a two dimensional terrain. 


the heading can be tracked by numerically integrating the yaw gyro output, 
&z 


W =¢, (1.14) 


The accelerometer outputs are not the north and east accelerations, but 
rather the longitudinal and lateral accelerations. Therefore these measure- 
ments must be rotated into the north-east frame by the heading angle. 


Ay =a, cos(W) - ay sin(y) (1.15) 


ae =ax sin() +a, cos(y) (1.16) 


w 


These accelerations can then be integrated to get the north and east 
velocities. 


Dy =Ay (1.17) 
Ve =A¢ (1.18) 
These velocities are then integrated to get the north and east positions. 
n=V,y (1.19) 


é =v, (1.20) 
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As an example, given the yaw gyro measurement shown in Figure 1.11, an 
initial heading of 0 degrees is integrated for the heading shown in Figure 1.12. 

Example accelerometer measurements are shown in Figure 1.13, which 
can be combined with the previously calculated heading to generate the ve- 
hicle position plot in Figure 1.14. 


1.3.4 Sensing Earth Effects 


Since the sensor is measuring the force applied to the proof mass, placing the 
sensor on a level surface in the Earth’s gravitation field will give a reading of 
approximately 9.81 ”/2 in the up direction. This is because the mass is experi- 
encing a force from the body of the sensor to keep it in place that counteracts 
the force due to gravity. If the sensitive axis of the accelerometer is in the 
direction of travel of a vehicle, a positive acceleration will give a measurement 
in the direction of travel since the body of the sensor is forcing the mass into 
motion in the direction of the vehicle. This concept is important in under- 
standing that the sensor does not measure “gravity,” but can register it due to 
the reaction forces applied to the proof mass. 
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Figure 1.11 Example yaw gyro output for turn maneuver. 
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Figure 1.12 Example heading for turn maneuver. 
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Figure 1.13 Example accelerometer measurements for turn maneuver. 
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Figure 1.14 Resulting calculated position for turn maneuver. 


This effect is also important to account for if any vehicle roll or pitch 
occurs. Since this happens frequently in vehicle operation, steps should be 
taken to compensate for this. A case of this error introduction is shown in 
Chapter 4. Due to unmodeled errors like the gravity direction, either a higher 
fidelity model is needed and/or inclusion of other sensors in the navigation 
solution. The latter method is discussed in detail in Chapter 4. 

Since the Earth is rotating with respect to some inertial frame, this ro- 
tation will be included in the measured rotations of the gyroscope. Unlike 
the gravity effect on accelerometers, however, the effect is usually considered 
negligible in MEMS devices since it is indistinguishable from the gyroscope 
bias and noise. Use of higher accuracy gyroscopes actually allow the measure- 
ment of this rotation rate and would therefore require inclusion of this effect 
in the sensor model. 


1.3.5 Inertial Instrument Errors 


Most inertial sensors suffer from similar error sources. These error sources 
may be the result of different causes but can result in the same error trends. 
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Inertial sensor errors are typically grouped by the systematic effect they have 
on the resulting measurement. The errors later considered in Chapter 4 
are the noise and bias errors. Other errors can be modeled and are briefly 


described. 


1.3.5.1 Noise 


The high-frequency variations of the inertial measurements are considered 
noise. Noise is constantly present in measurements of physical quantities due 
to many reasons. Unmodeled dynamic effects such as vehicle vibrations and 
thermal noise due to converting physical phenomenon to electric signals are 
examples of noise sources. Regardless of the source of the noise, its pres- 
ence in the measurement means that subsequent measurements of the same 
motion will not produce a consistent measurement. (One exception to this 
arises due to quantization error, described in more detail in Section 1.4.1.) 
The noise on an inertial sensor is typically considered white, zero mean, and 
Gaussian distributed so that simplified filtering techniques can be used in the 
processing of these measurements. Therefore a noisy sensor measurement can 
be modeled as 


s =x +v (1.21) 


where s is the measurement, x is the quantity measured, and V is the noise 
that is modeled as a zero mean Gaussian distributed white noise with variance 
O2. For an automotive grade accelerometers, this variance can be taken as 
le-2 m/s? Hz”, and for automotive grade gyroscopes, as 2.4e-7 rad/s” Hz” 
[16]. These values must be scaled by the sampling frequency since the 
measurements are used in discrete time. Inclusion of these errors in the ex- 
ample path in Section 1.3.3 gives position errors after 2 minutes shown in 
Figure 1.15. 


1.3.5.2 Bias 


Due to inaccurate calibration or slowly drifting error processes, the reported 
measurement may be biased from the true value. Drifting biases can result 
from unmodeled effects such as temperature variation. In such cases, the bias 
can change slowly over time as the temperature of the sensor changes. Bias 
removal is important in the application of inertial sensors since the reported 
quantity is integrated to relate to another navigation value. This error propa- 
gation is described in greater detail in Section 1.3.6. Often at least part of the 
bias can be considered to come from the same error mode as the noise. This 
violates the assumption of zero mean noise. Since the navigation algorithm 
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Figure 1.15 Final position error when automotive grade noise is included, sampled at 
10 Hz. 


requires zero mean noise, the bias is often estimated as part of the navigation 
state. This process is described in greater detail in Chapter 4. Accurate esti- 
mation of the bias allows the navigation system to remove the bias from the 
measurement and more closely approximate the Gaussian zero mean assump- 
tion. Therefore the sensor model can be extended with a bias 0. 


s =x +b 4+Vv (1.22) 


Another difficulty in implementing sensors that suffer with a bias is 
that often the bias does not remain constant over time. The bias stability is a 
factor that goes into selection of an IMU grade. Navigation grade IMUs have 
very slow bias drifts while low-cost sensors can have more significant changes 
in a short time period [17]. In many applications the time period over which 
the bias is used is short enough that this effect can be neglected. However, if 
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the bias drift is fast enough for the application, a nonconstant model should 
be used in estimating the bias. In this case, the bias can be modeled as driven 
by its own noise as 


b=uU (1.23) 


where [ is a zero mean Gaussian random variable with variance 67. Alterna- 
tively, the bias can be modeled as a Markov process with a model shown here. 


6 =— +v (1.24) 


Here T is the sensor time constant and V is a zero mean Gaussian ran- 
dom variable with variance 07. For an automotive grade gyroscope, values 
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Figure 1.16 Final position error with noise and bias error included in example path. 
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of 8.7e-4 ™/, and 300 s can be used for the noise standard deviation and 
time constant, respectively. For an automotive grade accelerometer, values of 
1.2e-2 ”/2 and 100 s can be used [16]. Addition of these errors along with 
noise gives final position errors for the example path in Section 1.3.3 shown 
in Figure 1.16. More detail of these processes is given in Chapter 4. 


1.3.5.3 Integration 


Two problems arise when integrating inertial sensors: the accumulating na- 
ture of integration and the unknown initialization. Any error that has previ- 
ously been introduced by either initialization or integration remains in the 
quantity being generated by the integration. As there are errors in the inte- 
grated quantity, this error is introduced into the propagated quantity. These 
errors will also be found in all subsequent values of the propagated value. For 
example, if imperfect velocity information is available to be integrated to find 
position, this error will cause an apparent drift in the position that will not 
necessarily be corrected by subsequent velocity errors. For example, given 
a velocity sensor that has some measurement noise, the integrated position 
could look like that in Figure 1.17. In this case, there is zero velocity and 
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Figure 1.17. Example of accumulated position error due to numerical integration of noisy 
velocity measurement. 
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the initial position is known, therefore the resulting reported position is the 
position error. 

Similarly, if the initial value of the propagated quantity is unknown, 
there is no way to determine it from the integrated quantity. This error will 
remain in all subsequent values of the propagated quantity. 


1.3.5.4 Other Errors 


While noise and bias are typically the major error contributors in the use of 
automotive grade inertial sensors, there are other effects that can be modeled 
in an inertial sensor’s output. Due to calibration error, there is often an un- 
known (though close to unity) scale factor between the actual quantity being 
measured and the output given by the sensor. For small variations in sensor 
output, this effect could appear as a bias. Therefore, large platform dynam- 
ics are necessary to effectively observe and estimate the scale factor errors on 
inertial sensors [18]. Improved estimation performance is possible if the scale 
factor can be estimated. However, due to the difficulty in dynamically excit- 
ing the sensor, the scale factor is sometimes assumed to be unity. 

Many IMU packages include sensors with orthogonal sensitive axes al- 
lowing for a single unit to measure motion in multiple directions. Errors in 
the orthogonality of the sensors can be estimated and then included whenever 
the sensor is used since this alignment will not change over time. Similarly, a 
mounted sensor may be misaligned from the assumed directions. These errors 
can be significant depending on the application. Temperature variations can also 
affect inertial sensor outputs but can be calibrated into the navigation system. 


1.3.6 Inertial Error Propagation 


Inertial sensors include deterministic and stochastic measurement errors so 
their unaided use as a navigation system will quickly degrade over time. As 
described earlier, integration of these errors causes error to grow in the other 
navigation states. Since the measurement noise has a stochastic part, it is 
impossible to predict exactly how this error will propagate. However, bounds 
on the growth of the error can be deterministically found based on sensor 
class and error type [17]. A bias in the integrated quantity will severely affect 
the performance of the navigation system. Therefore these biases must be 
calibrated out (in the case of high fidelity inertial sensors) or estimated out 
(using alternative sensors). 

Even with deterministic errors removed, the propagation due to inte- 
gration of a noisy measurement yields increasing uncertainty. Figure 1.18 
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Figure 1.18 Error propagation of single-integrated white noise. 


shows one such bound for single-integrated white noise. This type of error is 
observed when using an integrated accelerometer to calculate velocity or an 
integrated gyro to calculate heading. The bounds are a function of the vari- 
ance of the sensor noise (67), the sampling period (6¢), and the number of 
integration steps (A) as 


o? =07 61 & (1.25) 


However, this error growth does not include unremoved bias, scale fac- 
tor, misalignment, and other errors. Each of these error sources would in- 
crease the growth rate of uncertainty. 

Specification sheets distributed by inertial sensor manufactures give 
values that correspond to the accuracy of the sensor by describing how these 
errors are distributed [17]. 


1.4 Odometer Technology 


Information about speed and heading change can be taken by measuring 
wheel speeds. Therefore, odometer technology can be used as additional 
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sensors for vehicle applications. This is particularly useful as these sensors are 
not dependent on surrounding conditions other than the tire-surface contact. 
There are several types of odometers that may be lumped into active and 
passive groupings. Both of these types have advantages and disadvantages in 
their use. Active sensors include Hall-effect transducers that can operate well 
at low speeds. Passive sensors require sufficient relative motion between the 
sensor teeth to generate a signal [19]. For other applications, optical encoders 
may be more appropriate such as in robotic platforms. All of these modules 
use the fact that given a constant wheel radius, the distance traveled is the 
product of the radius and the rotation in radians. The wheel speed is also the 
radius times the wheel rotation rate in radians per second. Using these values 
in addition to GPS, the variations in GPS can be filtered and changes in the 
wheel radius can be monitored [19]. 

There are several specific applications for odometry use in vehicle or 
tracked vehicle control. Just as inertial sensors were used to dead reckon, 
wheel sensors can be used to dead reckon a vehicle’s position and orientation 
given an initialization. Particularly when used with GPS, the wheel speeds 
can be used to update the vehicle velocity and also indicate a static vehicle 
so that sensor biases can be estimated with static assumptions. The measure- 
ments provided by odometers are taken in the axis of the wheel, which may 
be in a frame that is not coincident with the vehicle frame. Therefore, other 
effects may be required to relate wheel rotations to motion detected by other 
sensors. 


1.4.1. Quantization 


Quantization is a common error in any sensing situation in which continu- 
ous values are translated into finite-precision values. Due to the nature of 
the odometer, there are infinitely many physical angles that result in a single 
quantized measurement. This effect is most noticeable when the step size in 
the odometer is larger than the precision available in the discretization. Com- 
parison of quantized output versus true quantity is shown in Figure 1.19. 


1.4.2 Wheel Slip 


Another error that will affect the accuracy of odometers is wheel slip. Al- 
though some wheel slip is expected in most normal operation [20], there are 
cases of catastrophic slip in which the motion of the wheel in contact with 
the ground is not acting to move the vehicle. This is the case when sliding, 
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Figure 1.19 Quantized output of a continuous value. 


spinout, and so forth occur on surfaces like ice and gravel. This will affect the 
resulting dead reckoned solution from the wheel sensors since there is more 
angular change than positional change. Other than these cases, some amount 
of slip is expected, particularly with drive wheels. Since slip is often quoted as 
a percentage, the amount of slip acts as a scale factor error on the odometer. 
This error type often changes quickly and can be a function of wheel speed 
and drive force. Estimation of this quantity can be accomplished with other 
sensors such as GPS [21]. 


1.4.3 Wheel Radius Error 


Similar to the wheel slip error, an error in the wheel radius will act as a scale 
factor on the odometry measurement. Assuming this error is slowly chang- 
ing, this can be estimated by the navigation filter in ways similar to the biases 
and drift rates of the inertial sensors. Since rapid changes to wheel radius can 
occur (as in a flat tire), there can be resulting large changes in the odometry 
error. 
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1.5 GNSS/Inertial Integration 


Due to the complementary nature of GNSS and inertial sensors, their com- 
bination can yield improved navigation results. The result is a navigation 
system that builds on the strengths of each sensor type and mitigates the 
errors inherent in them. Typically the combination is in the form of an ex- 
tended Kalman filter (EKF) although many alternative implementations have 
been investigated [22]. Many types of integration can be made with several 
combinations of sensor measurements. An example case (and perhaps the 
most straightforward) is a loosely coupled EKF. Here the state would include 
some attitude reference (i.e., heading if dealing with a planar case), veloc- 
ity states, position states, and augmented states to account for sensor biases, 
drifts, and so forth. The inertial sensors are used to propagate the navigation 
portion of these states since their measurements are of the derivatives of these 
states. Gyroscopes are used to propagate attitude angles as shown in (1.26) 
and accelerometers are used to propagate velocities in (1.27), each with their 
respective error terms. 


g=W tb, +V, (1.26) 


a= +b, +V, (127) 


In these equations, (YW) represents an attitude angle, (v) represents ve- 
locity in the accelerometer axis, (6) represents the sensor bias drift, and (Vv) 
represents the sensor noise. Since the bias terms are included in the states, 
their rate-of-change models are also included in this propagation. Traditional 
models include simple constants or a Gauss-Markov processes to model a 
slowly drifting bias. Modeling the bias as a Gauss-Markov process allows the 
model to handle the drift of the bias. The bias can be modeled as propagating 
according to (1.24). 

Along with the propagation of the states, the estimate of the state cova- 
riance is also propagated in this step using the models of the states (in state 
transition matrix form). Note that these covariances are simply the estimates 
of the covariances and not the true. Their proximity to the true covariances 
is based on several factors including model accuracy and accurate sensor 
variances. 

This state propagation can be performed indefinitely but the resulting 
covariance (and true covariance) will grow without bound since no correc- 
tion will be applied to the state. To remedy this, periodic measurement up- 
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Figure 1.20 Flowchart of a loosely coupled system. 


dates are made in which GPS and/or odometry sensors are used to measure 
state values. These sensors also have error terms inherent in their use that are 
typically modeled as additive noise. The propagated state is used to generate 
the expected measurement that is then differenced with the actual measure- 
ment taken. This difference is called the innovation, which is effectively the 
amount of new information made available by the measurements. The propa- 
gated state covariance is then used to generate a weighting (called the Kalman 
gain) to transfer the measurement innovation into state corrections. Once 
the states have been corrected, the state covariance is then updated to reflect 
this correction. The process is repeated, giving a rapidly updated navigation 
solution (due to the state propagation) that is corrected (by the measurement 
update). The general flow of the filter is shown in Figure 1.20. Subsequent 
chapters will show performance of this sensor combination. While there are 
many other GNSS/inertial combination methods, they are beyond the scope 
of this text and the reader is referred to other works [13, 15]. 
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Vision Aided Navigation Systems 
John Allen 


Many vehicle manufacturers have developed lane departure warning (LDW) 
systems to reduce the number of traffic fatalities that occur due to unintentional 
lane departures. LDW systems alert the driver when the vehicle has driven 
outside the lane markings of the current lane of travel. Most of the LDW sys- 
tems in production now are solely based off camera measurements. An LDW 
camera uses feature extraction to determine lateral position in the current lane. 
The feature used for a camera-based LDW system is the painted lane lines. 

Lane positioning can also be accomplished using a light detection and 
ranging (Lidar) scanner. Unlike the camera, a Lidar scanner can provide 
three-dimensional ranging information; however, Lidar does not provide 
color information. To overcome this, some Lidar scanners also provide re- 
flectivity data. The reflectivity data can be used to extract lane markings; and 
the ranging information can be used to provide an estimate of a vehicle’s 
lateral position in the lane. 

The objective of this chapter is to provide a method of using a lateral 
lane position measurement to aid a traditional navigation system [1-5]. Two 
types of vision integration are discussed in this chapter. The first is integra- 
tion of vision to aid a position, speed, and heading navigation filter. The 
second is integration of vision to aid a closely coupled navigation filter. Both 
of these filter architectures are discussed in detail in Chapter 1. 
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This chapter will start with a brief overview of how the camera and 
Lidar are used to measure a vehicles lateral position in a lane. Next, the nec- 
essary coordinate frame rotations are discussed. Two types of rotations are 
covered. The first rotation is a two-dimensional rotation that is necessary for 
integration with the position, speed, and heading navigation filter. The sec- 
ond rotation is a three-dimensional rotation that is necessary for integration 
with the closely coupled navigation filter. The next section will discuss vision 
integration with the position, speed, and heading navigation filter. The final 
section of the chapter will discuss vision integration with the closely coupled 
navigation filter. 


2.1__Lane Positioning Methods 


Two methods of lane positioning are presented in this section. The first is 
using a Lidar for lane positioning. Currently, Lidar scanners are very expen- 
sive. The cost of Lidar scanners has prevented implementation of Lidar-based 
LDW systems on civilian vehicles; however, the cost of Lidar scanners is ex- 
pected to decrease as demand for these devices increases. The second method 
of lane positioning involves using a camera. Ideally, both cameras and Li- 
dar scanners will be used in future LDW systems. Each device has different 
strengths and weaknesses. Using both devices will improve the robustness of 
both the LDW system and navigation system aiding. 


2.1.1 Lidar-Based Positioning 


Lidar measures the range to an object by pulsing a light wave at the object 
and measuring the time between transmission and reception. The light wave 
for Lidar applications is a laser. Lidar is very similar to sonar, but instead of 
using a sound wave, Lidar uses a light wave. Lidar scanners combine the laser 
with a moving mirror that rotates the laser’s beam. This can provide ranging 
information in multiple directions, both vertically and horizontally. Lidar 
can provide reflectivity measurements, a measurement known as echo width. 
Reflectivity data can be used to classify objects that the Lidar encounters. 

A Lidar scanner with reflectivity measurements can be used to search 
for and range off of lane markings. Painted lane markings are more reflective 
than the asphalt that surrounds the lane marking, therefore the echo width 
data from a Lidar scanner can be used to find lane markings, and the ranging 
information from the Lidar can then be used to determine the vehicles lateral 
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Figure 2.1 Visual representation of a Lidar scanner searching for lane markings. 


distance from the lane marking. Figure 2.1 shows a Lidar scanner mounted 
atop a vehicle and a representation of its scan pattern. Figure 2.2 shows a 
sample reflectivity data from a Lidar scanner, and Figure 2.3 shows a picture 
taken from a camera aligned with the Lidar scanner. 

The distinct spikes in the echo width are a result of the laser hitting a 
lane marking. At the start and end of the laser scan, when the laser is not con- 
tacting the road, the echo width measurements become noisy. This will result 
in spikes in the echo width that do not correspond to a lane marking. There- 
fore, it is necessary to use search bounds on the echo width measurements. 


Echo width 
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Figure 2.2 Average echo width for 100 scans. (From: [1].) 
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Figure 2.3. Corresponding echo width to lane markings. (From: [1].) 


Testing has shown that the standard deviation of a Lidar-based lane position 
measurement under ideal circumstances is around 0.044 meter [1]. 

One advantage of using a Lidar scanner—based LDW system is the ro- 
bustness of Lidar scanners to varying lighting and weather conditions. Unlike 
a camera, Lidar scanners work independent of surrounding lighting condi- 
tions. Currently, the largest disadvantage of Lidar-based LDW systems is the 
cost of the hardware. Lidar is a relatively new technology with a limited num- 
ber of manufacturers. Also, using Lidar scanners to detect lane markings is a 
new and undeveloped science. 


2.1.2 Camera-Based Positioning 


Cameras are the most popular type of hardware used to determine lane pos- 
ition for LDW systems. Camera-based LDW systems are available as an 
option on some production vehicles. Many methods of camera-based lane 
detection are present in the literature; the following is one method. The first 
step of camera-based lane positioning is lane marking extraction. Thresh- 
olding is the process of filtering out unwanted features in the image and is 
used to extract desired areas of the image such as lane markings. Then, edge 
detection and the Hough transform are used to extract areas of the image 
that have lines or edges, as in Figure 2.4. Other camera-based lane detection 
systems employ optical flow, neural networks, and alternatives to the Hough 
transform such as peak and edge finding. The vehicles position in the lane 
can be estimated after the image is searched for lane markings. One simple 
method of estimating lateral lane position is counting the number of pixels 
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Figure 2.4 Lane lines extracted from the Hough transform. (From: [4].) 


from the center of the image to the lane marking at the bottom of the image. 
The number of pixels is multiplied by a scale factor to determine the dis- 
tance from the center of the vehicle to the lane line. Testing has shown that 
standard deviation of a camera-based lane position measurement under ideal 
circumstances is around 0.059 meter [2]. 

One advantage of using a camera-based LDW system is the cost of the 
hardware involved. Digital cameras have been in production for decades, and 
the cost of these devices is relatively cheap. Also, methods of lane positioning 
using a camera were pioneered in the 1990s; therefore, the algorithms used 
for lane positioning using a camera are well established. Some disadvantages 
to camera-based LDW systems include vulnerability to lighting and weather 
conditions. At dawn and dusk, when the sun is low in the sky, a camera may 
be blinded by the sun. Also, camera-based lane detection can be difficult in 
urban environments where lane markings are in poor condition or visibility 
of lane markings are blocked by surrounding traffic. 


2.2 Coordinate Frame Rotation and Translation 


Lane position measurements are given in the road-based coordinate frame. 
The road-based coordinate frame can be approximated with a waypoint- 
based map. A rotation matrix is needed to rotate coordinates in the naviga- 
tion’s coordinate frame to the road coordinate frame because the navigation 
filter’s coordinate frame is not oriented the same way as the road coordi- 
nate frame. Since the navigation coordinate frame and the road coordinate 
frame do not have the same origin, coordinate frame translation must also 
be covered. 

A rotation matrix is a matrix that if multiplied by a vector of values ex- 
pressed in an initial coordinate frame will result in a vector of values expressed 
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in a new coordinate frame. The new coordinate frame has some different at- 
titude from the initial coordinate frame. The difference in attitude will gov- 
ern the values in the rotation matrix. A rotation matrix can be constructed 
using Euler angles. The rotational direction of Euler angles are based off the 
right-handed coordinate system. In short, rotation about an axis that points 
towards the observer results in a counterclockwise-positive rotation. Rota- 
tion about an axis that points away from the observer results in a clockwise- 
positive rotation. Rotation matrices are orthogonal; therefore, the inverse 
of a rotation matrix is equal to its transpose. Equation (2.1) describes this 
principle. 


CB" =c 8 =0¢ (2.1) 


Rotation matrices are denoted by C8. This rotation matrix maps coor- 
dinates in the @ coordinate frame to the B coordinate frame. e denotes the 
ECEF coordinate frame, 7 denotes the North East coordinate frame, and r 
denotes the road coordinate frame. 


2.2.1. Two-Dimensional Rotations 


The position, speed, and heading navigation filter employs a two-dimen- 
sional navigation coordinate frame. Two-dimensional coordinate frame rota- 
tion matrices are 2 2 matrices based off one attitude angle. Figure 2.5 shows 
the principle of a two-dimensional rotation. 


Figure 2.5 A rotation matrix (2.2) can be constructed to map vectors in the initial coor- 
dinate frame (ij) to a rotated coordinate frame (/’,/’). Theta is the angle of 
rotation. This angle is measured from the / axis to the /’ axis. The right-hand 
coordinate frame rule governs which direction of rotation is positive. The angle 
theta in this picture represents a positive rotation. 
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The (2,7) coordinate frame is the starting coordinate frame, and the 
(z’,j’) coordinate frame is the final coordinate frame. For the coordinate 
frame in Figure 2.5, the right-handed coordinate frame rule defines the 
axis of rotation as pointing down (or away from the observer); therefore, 
clockwise rotation results in a positive Euler angle. Equation (2.2) shows 
the rotation matrix that will map coordinates in the (2,7) coordinate frame 
to the (z’,7’) coordinate frame. The rotation matrix used to aid the position, 
speed, and heading navigation filter will use a rotation matrix of the same 
format. 


(i¢jd —écos(9@)_ sin) ¢ 
(ij) ~ @ sin(@) cos(@)C (2.2) 


2.2.2 Three-Dimensional Rotations 


Three-dimensional coordinate frame rotation matrices are 3. 3 matrices 
based off three Euler angles. A three-dimensional coordinate frame rotation 
can be thought of as a series of three two-dimensional rotations. Figure 2.6 
shows the rotation series used in this book. 

The first rotation is about the z-(&) axis. The second rotation is about 
the new y-(/’) axis. The third rotation is about the new x (2”) axis. Equation 
(2.3) shows how the three-dimensional rotation matrix is constructed. s, is 
the sine of 0, and c, is the cosine of 0), sy is the sine of ,, and cy is the cosine 
of 0), s3 is the sine of 03, and c; is the cosine of 03. 


Figure 2.6 The first rotation is about the k axis. This will result in a new coordinate frame 
(/,j',k’). The second rotation is about the /’ axis. This will result in a new coor- 


mon 


the final rotated coordinate frame. 
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2.2.3 Coordinate Frame Translation 


The origin of the road-based coordinate frame is not located at the same 
point in space as the origin of the navigation coordinate frame origin. When 
mapping coordinates in the navigation coordinate frame to the road coor- 
dinate frame, the coordinate frame must be moved and rotated. Figure 2.7 
shows an example of a coordinate frame translation and rotation. 

Equation (2.4) shows how to map coordinates in coordinate frame that 
has been moved and rotated %/, is the position vector expressed in the initial 
coordinate frame (@), and 7%, is the position vector expressed in the rotated 


Figure 2.7 An example of a two-dimensional coordinate frame translation and rotation. In 
order to map a position vector in the alpha coordinate frame to the beta coordi- 
nate frame, two things must be known. The first is the position vector from the 
origin of the alpha coordinate frame to the origin of the beta coordinate frame 
expressed in the alpha coordinate frame. The second is the change in attitude 
from the alpha coordinate frame to the beta coordinate frame. 
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and translated coordinate frame (B). 74, is a vector expressed in the initial 
coordinate that points from the initial coordinate frame to the new coordi- 
nate frame, and cB is the rotation matrix from coordinate frame @ to coor- 
dinate frame P. 


Fy =Ch Gly ~ Fash (2.4) 


Coordinates in the final coordinate frame can be mapped back to the 
initial coordinate frame using (2.5). 


Fiky =CBTh, +g (2.5) 


2.2.4 Global Coordinate Frame Rotations 


Sometimes it may be necessary to map coordinates in the Earth Centered 
Earth Fixed (ECEF) coordinate frame to the North East Down (NED) 
coordinate frame. The north-east plane of the NED coordinate frame is 
tangential to a reference ellipsoid. The reference ellipsoid is an ellipsoid 
that mimics the surface of the Earth. Using the latitude (@) and longi- 
tude (A) of the origin of the NED coordinate frame, a rotation matrix that 
maps ECEF coordinates to NED coordinates can be constructed. Equation 
(2.6) shows the rotation matrix that maps coordinates in the ECEF coordi- 
nate frame to the NED coordinate frame. The transpose of this matrix will 
map coordinates from the NED coordinate frame to the ECEF coordinate 
frame. 


& sin(d) cos(A) —_- sin(@)sin(A) cos(@) 
- sin(A) cos(A) 0 
é cos(@) cos(A) —- cos(@) sin(A) —_- sin(@) 


crs (2.6) 


(D> (D> 
(aan ane on] 


Equation (2.7) coverts ECEF position coordinates (Fp) to NED posi- 
tion coordinates (7,,). Notice that, along with the latitude and longitude of 
the origin of the NED coordinate frame, the position of the origin expressed 
in the ECEF coordinate frame (7) must be known. 


rh =C! eS - ast (2.7) 
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Equation (2.8) coverts NED position coordinates to ECEF position 
coordinates. 


T 
fp =Cl" ip 2.8) 


Equation (2.9) coverts ECEF velocity coordinates to NED velocity co- 
ordinates. Notice that velocity mapping is independent of the position of the 
NED coordinate frame. The latitude and longitude of the NED coordinate 
frame is still need to construct the rotation matrix. 


Dn =Ce 0, 2.9) 


Equation (2.10) coverts NED velocity coordinates to ECEF velocity 
coordinates. 


T 
B,. =C. 3b, (2.10) 


2.3 Waypoint-Based Maps 


The road coordinate frame is the coordinate frame in which the vision mea- 
surements are given. The road frame can be approximated using a waypoint 
map. The waypoints lie in the center of the lane that is being mapped. The 
distance between waypoints should be defined by the road geometry. Com- 
plex road geometry will require waypoints to be close together. For example, 
in a turn, the waypoints will need to be close together to match the geometry 
of the road. On a straightaway, the waypoints can be very spread out due to 
the lack of change in road geometry. 

The position states of the navigation filter must be mapped into the 
road coordinate frame. Equation (2.4) can be used to map position from 
the navigation coordinate frame to the road coordinate frame. In order to 
use this equation, the position of the base waypoint in the navigation coor- 
dinate frame must be known. Also, the rotation matrix from the navigation 
coordinate frame to the road coordinate frame must be known. In order to 
construct this rotation matrix, the attitude of the road coordinate frame with 
respect to the navigation coordinate frame must be known. These elements 
can be saved in a map database. 

When the navigation filter is initialized, it must check to see where on 
the map the vehicle is located. GPS can be used to initialize the vehicle’s posi- 
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tion in the navigation coordinate frame. This position can be used to search 
the map database. 

The distance from the base waypoint to the next waypoint can also be 
stored in the map database. This value is useful for checking if the vehicle has 
passed the next waypoint. 


2.4 Aiding Position, Speed, and Heading Navigation Filter with 
Vision Measurements 


The position, speed, and heading navigation system is a three-degree-of- 
freedom navigation filter based in a two-dimensional, rectangular coordinate 
frame. The navigation coordinate frame for this filter is the north-east coor- 
dinate frame. The north-east coordinate frame is denoted by v. The x-axis of 
the navigation coordinate frame points north and the y-axis of the coordinate 
frame points east. The road coordinate frame for the position, speed, and 
heading navigation system is also a two-dimensional coordinate frame. The 
road coordinate frame is denoted by 7. The x-axis of the road coordinate 
frame points from the last waypoint passed to the next waypoint. The y-axis 
of the road coordinate frame is perpendicular to the x-axis. If facing with the 
x-axis, the y-axis points to the left. 

Equation (2.11) shows an example map database that can be used to 
aid the position, speed, and heading navigation system. N,; and E,,; is the 
position coordinates of the ith waypoint. y,,; is the heading of the ith road 
coordinate frame. The heading of the road coordinate frame is measured 
from the north axis with clockwise rotation being positive. d,; is the distance 
from waypoint 7 to waypoint 7 + 1. This value is used to check if the vehicle 
has passed the next waypoint. Figure 2.8 depicts a portion of this database 
graphically. 
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The rotation matrix in (2.12) represents the rotation matrix that maps 
coordinates in the north-east (navigation) coordinate frame to the road co- 
ordinate frame. The angle y,; is the heading of the ith road coordinate 
frame. This angle should not be confused with the heading state of the 
filter. 
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Figure 2.8 Visual example of a two-dimensional waypoint map. 


, &cos(y,) sin(y,) t 
Cn =6— \ 
& sin(W,) cos(W,)¢ 


(2.12) 


2.4.1 Two-Dimensional Map Construction 


In order to aid the position, speed, and heading navigation filter with lane po- 
sition measurements, the position of the waypoints (J, ;, £,,;) and the heading 
(y,,;) and distance (d, ;) between waypoints must be known. The positions of 
the waypoints (/,.;, £, ;) are assumed to be given through a survey. The head- 


ing between waypoints can be solved with (2.13). 
W,j; =atan2 ([E,e4 = Eri LN, in = N,,]} (2.13) 


The distance between the waypoints can be determined using the dis- 
tance equation. 
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(E44 > Ej) HN, 541 < Nyy (2.14) 


2.4.2 Measurement Structure 


In order to use lateral lane position measurements, the lateral lane position 
with respect to the lane map must be estimated using the current states 
of the navigation filter. N and £ denote the current position estimate in 
the north east coordinate frame. Np and Ep denote the position of the last 
waypoint passed in the north-east coordinate frame. <and 7 denote the esti- 
mated position in the road coordinate frame. Equation (2.15) is used to find 
the position estimates in the road coordinate frame. 


au ev-Nn,t 
A.U=Cr &, U (2.15) 
evu ee - E, ¢ 


Equation (2.16) shows the measurement equation. The measurement 
equation is a function of the states of the filter and the map parameters. The 
matrix in (2.17) shows the measurement model, assuming the state matrix 
takes the form of (4.85) in Chapter 4. The measurement model is created 
by taking the partial derivative of the measurement equation with respect to 
each state. ,; is the heading of the current (th) road coordinate frame, not 
the heading state of the filter. 


A(x) =j =-(N - N,s)sin(W,i) HE - Ey.) cos(Wr.i) (2.16) 


H(x)=[0 0 0 0 -sin(y,,) cos(y;,)] (2.17) 


2.4.3 Checking Waypoint Map Position 


After every measurement update and time update, the states of the navigation 
filter will change. Whenever the states of the filter are updated, the distance 
into the current road frame must be checked to ensure that the vehicle has not 
passed into the next road coordinate frame. Substituting the position states 
into (2.18) will result in the current longitudinal distance into the road coor- 
dinate frame. If this value is larger than d, ;, then the vehicle has passed into the 
next coordinate frame. The map index 7 should then be incremented by one. 


52 GNSS for Vehicle Control 


& =(N - N,,;)cos(Wy,;) HE - E,,;)sin(W,) (2.18) 


2.4.4 Results 


Figure 2.9 shows the utility of using vision to aid a GPS-based navigation fil- 
ter. Figure 2.9 is a plot of the lateral lane position estimates. This data for these 
results was collected at the NCAT test track, which is a 1.8-mile, two-lane 
oval track. The track is set up very similar to the interstate system. The thin 
line represents the GPS measurements. GPS position is a bias measurement. 
These biases make it difficult to compare a GPS position measurement and 
a map to get vehicle position within a lane. The thick line is the result of the 
vision aided navigation filter. Unlike the GPS measurement, the estimated 
lane position reported by the vision aided navigation filter is not biased. 


2.5 Aiding Closely Coupled Navigation Filter with 
Vision Measurements 


The closely coupled navigation filter is a six-degree-of-freedom navigation fil- 
ter. The filter tracks the global position and velocity of a vehicle. The position 
and velocity estimates are based in the Earth centered earth fixed (ECEF) coor- 
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Figure 2.9 Estimated lane position reported by GPS and the vision aided navigation filter. 
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dinate frame. The ECEF coordinate frame is a three-degree-of-freedom coordi- 
nate frame and is denoted by e. The closely coupled navigation filter also tracks 
the attitude of the vehicle. The attitude is represented by the three Euler angles. 
Along with the pose of the vehicle, the navigation filter tracks accelerometer 
biases, gyro biases, GPS clock bias, and GPS clock drift. A more in-depth look 
at the structure of closely coupled navigation filter can be found in Chapter 3. 
The goal of this section is to describe how a lateral lane position mea- 
surement can be incorporated into the closely coupled navigation filter. Lateral 
lane measurements from a camera or Lidar can aid the lane navigation solu- 
tion and provide robust positioning even under limited GPS satellite visibility 
such as in urban canyons [5]. Similar to the position speed and heading filter, 
a road map is necessary to include lane position measurements. The road co- 
ordinate frame for the closely coupled navigation filter is similar to the road 
coordinate frame for the position, velocity, heading filter. The road coordinate 
frame for the closely coupled navigation filter is a three-degree-of-freedom 
coordinate frame and is denoted by 7. The x-axis of the road coordinate frame 
points from the last waypoint passed to the next waypoint. The y-axis of the 
road coordinate frame is perpendicular to the x-axis. If facing the x-axis, the y- 
axis points to the left. The road coordinate frame is assumed to have no bank; 
therefore, the y-axis is always parallel with the plane tangent to Earth’s refer- 
ence ellipsoid. The z-axis is perpendicular to the x-y plane and points down. 
Equation (2.19) represents a example map database that can be used. 7; 
is the position vector of waypoint 7 in the ECEF coordinate frame. @; is the at- 
titude vector of the road coordinate frame. The attitude is represented the same 
way as the attitude states of the vehicle, with three Euler angles. d,; is the dis- 
tance from waypoint 7 to waypoint 7 + 1. This value is used to check if the ve- 
hicle has passed the next waypoint. Figure 2.10 depicts this database visually. 


ger @ 41 : 

Map Database =€ : pee (2.19) 
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The form of the rotation matrix from the ECEF coordinate frame to 
the road coordinate frame is given in (2.20). The elements from the road 
coordinate frame attitude (@,) are used to construct the rotation matrix. c, is 
the cosine of the first attitude angle in the attitude vector, and s, is the sine 
of the first attitude angel in the attitude vector. Similarly, c, and s) are the 
trigonometric functions of the second angle, and c3 and s3 are the trigionmec 
functions of the third angle. 
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Figure 2.10 Visual example of the three-dimensional road coordinate frame. 
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2.5.1 Three-Dimensional Map Construction 


The rotation matrix in (2.20) can be thought of as a sequence of two rota- 
tions. The first rotation is from the ECEF coordinate frame to the north, east, 
down (NED) coordinate frame (C’). This rotation matrix is based off two 
angles. @; is the latitude of the th coordinate frame, A, and is the latitude of 
the #th coordinate frame. The origin of the NED coordinate frame is at the 
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ith waypoint (7,,;). The second rotation is from the NED coordinate frame 
to the road coordinate frame (C’). This rotation matrix is also based off two 
angles. y; is the heading of the zth coordinate frame. @, is the pitch of the th 


coordinate frame. 


Cr =crce (2.21) 


Equation (2.22) shows the form of the rotation matrix that maps co- 
ordinates in the ECEF coordinate frame to the NED coordinate frame. 
The longitude and latitude of the ith road coordinate frame correspond 
to the global position of the ith waypoint. The latitude and longitude can 
either be surveyed, or solved for using the position of the zth waypoint. 
Transformations between ECEF coordinates and geodetic coordinates can 


be found in [2]. 


& sin(@;)cos(A;) —-- sin(@,)sin(A;) —_cos(@,) L 


Cc =§ - sin(A;) cos(A;) 0 { (2.22) 
& cos(,)cos(A;) - cos(9;)sin(A;)_— - sin(O,)k 
&os(0;)cos(W;) cos(0;)sin(y;) - sin(6;)t 

Cr=& - sin(yi) cos(W;) 0 — (2.23) 


&in(O;)cos(w;) — sin(@;)sin(w;) _—_cos(6;) f 


Equation (2.23) shows the form of the rotation matrix that maps co- 
ordinates in the NED coordinate frame to the road coordinate frame. The 
pitch and heading angles can be solved by looking at the change in position 
between waypoint 7 and waypoint i + 1. The first step in solving for these 
angles is to express the position of the 7 + 1 waypoint in the NED coordinate 
frame based at the ith waypoint. This can be done using (2.24). 


BE i = Ce Bein Hab (2.24) 
é ryt u_ e Beri ~ Ver : 


&D,.i bf 


Once the position of waypoint 7 + 1 is expressed in the NED coordinate 
frame based at coordinate frame J, (2.25) and (2.26) can be used to solve for 
the road coordinate frame heading and pitch. 
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W; =atan2(E,;,N,i) (2.25) 


é 
0; =atan ct! __s (2.26) 
E N7i +E B 


The longitude (A,), latitude (@,), pitch (0), and heading (y;) are all 
plugged into their corresponding rotation matrices. The result of (2.21) 
will be a rotation matrix that maps coordinates in the ECEF coordinate 
frame to the road coordinate frame. The three attitude angles in the map 
database (@;) can be extracted from this rotation matrix. Equations (2.27) 
through (2.29) show how to solve for the road coordinate frame attitude 
using the longitude, latitude, pitch, and heading angles. The relationship 
between attitude angles and rotation matrices is derived in [3]. 


Q; =atan2(- cos(@;)sin(Y;),cos(@;)sin(9;)cos(y;) - sin(;)cos(O;)) 
(2.27) 


Qj,2 =asin(cos(o;)cos(O;)cos(w;) +sin(@; )sin(O; )) (2.28) 


@;,3 =atan2(- sin(@;)sin(A;),cos(O;)cos(y;) +cos(A;)cos(6;)sin(y;) 
+cos(@;)sin(A;) sin(6;),- sin(@;)cos(A;) cos(8;)cos(Y%;) 


- sin(A;)cos(0; )sin( y;) +cos(@;)cos(A;)sin(@;)) 
(2.29) 


The distance between the waypoints can be determined using the dis- 
tance equation. 


yj = JE in - 73)! Fin - 72) (2.30) 


2.5.2 Measurement Structure 


In order to use lateral lane position measurements, the lateral lane position 
with respect to the lane map must be estimated using the current states of 
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the navigation filter. 7, denotes the current position estimate in the ECEF 
coordinate frame, %;,; denotes the position of the last waypoint passed in 
the north east coordinate frame, and C” is the rotation matrix given in 
(2.20). Equation (2.31) is used to find the position estimates in the road 
coordinate frame. # is the lateral lane position estimate, % is the distance into 
the current road coordinate frame, and Z is the vertical position in the cur- 
rent road coordinate frame. The z-axis of the road coordinate frame points 
down. 


eu 

e,.u A — ~ 

A) yale Fs : Teri (2.31) 
eu 


Equation (2.32) shows the measurement equations. The measurement 
equations are a function of the states of the filter and the map parameters. The 
first measurement equation is for the lateral lane position. This measurement 
is assumed to be provided by a camera or Lidar. The second measurement 
equation is the height above the road coordinate frame. Since this navigation 
filter structure is for a ground vehicle, this measurement can be assumed to 
be a constant. The matrix in (2.35) shows the measurement model, assuming 
the state matrix takes the form of (4.134) in Chapter 4. The measurement 
model is created by taking the partial derivative of the measurement equa- 
tions with respect to each state. 


A(x) = 


r ae el, r se el r me el t 
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(2.35) 


2.5.3 Checking Waypoint Map Position 


After every measurement update and time update, the states of the navigation 
filter will change. Every time the states of the filter are updated, the distance 
into the current road frame must be checked to insure that the vehicle has not 
passed into the next road coordinate frame. Substituting the position states 
into (2.36) will result in the current longitudinal distance into the road coor- 
dinate frame. If this value is larger than d, ;, then the vehicle has passed into the 
next coordinate frame. The map index 7 should then be incremented by 1. 


£ SC y (FS, - ia) +Clu.a (F5.9 - Her) +Clus) (F5- Heal 
(2.36) 
2.5.4 Results 


Figure 2.11 shows the utility of using vision to aid a GPS-based navigation 
filter. Figure 2.11 is a plot of the lateral lane position estimates. This data for 
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Figure 2.11 Estimated lane position reported by GPS and the vision aided navigation filter. 


Vision Aided Navigation Systems 59 


these results was collected at the NCAT test track, which is a 1.7-mile, two- 
lane oval. The track is set up very similar to the interstate system. The large 
line represents the GPS measurements. GPS position is a bias measurement. 
These biases make it difficult to compare a GPS position measurement and 
a map to get vehicle position within a lane. The small line is the result of the 
vision aided navigation filter. Unlike the GPS measurement, the estimated 
lane position reported by the vision aided navigation filter is not biased [5]. 
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Vehicle Modeling 


Lowell Brown and Dustin Edwards 


3.1 Introduction 


The goal of any model is to describe physical phenomena. Our interest is 
vehicular dynamics. There are several models that exist for describing vehicle 
motion [1-16]. We will explore a few of them. First, though, conventions 
must be established. 


3.2 SAE Vehicle Coordinates 


The Society of Automotive Engineers has established the SAE Vehicle Coor- 
dinate System. This is the system that will be used for body fixed coordinates. 
As shown in Figure 3.1 this coordinate system defines the longitudinal axis of 
the vehicle as x, the lateral axis to be y, and the vertical axis, z, points toward 
the ground. The coordinate system also defines the direction of roll rate p, 
pitch rate g, and yaw rate r. 

The body fixed SAE coordinate systems is attached to a vehicle that 
moves about the East, North, Up (ENU) global reference frame, as shown 
in Figure 3.2. 

In Figure 3.2 the horizontal axis is the East direction and vertical axis 
is the North direction. The positive Up direction comes out pointed at the 
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N <t-----[--5---+ 


Figure 3.1 SAE Vehicle Coordinate System defined by the SAE [1]. 


reader and is perpendicular to the E-N plane. The box represents a vehicle 
with V,and V, velocity components. Vehicle heading is y, the angle between 
a line parallel to the North axis and the longitudinal velocity component V,.. 
Sideslip, B, is defined as the angle between the magnitude of velocity and 
the longitudinal component of velocity, V,. Vehicle course, the direction of 
travel, is defined as v, where 


v=yw tp (3.1) 


Note that y is equivalent to 7, the yaw rate. 

Most models in this chapter will be based off the basic “bicycle model.” 
Other models are developed to capture dynamics the bicycle model is unable 
to describe. Also, simplifications are made to the bicycle model that are valid 


Figure 3.2 ENU global reference plane. 
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during certain steady-state maneuvers. The models developed in this chapter 
are important in later sections when developing parameter estimation algo- 
rithms and controlling a vehicle autonomously. Therefore, it is important to 
know the limitations and accuracy of each model. 


3.3. Bicycle Model 


3.3.1 Basics 


3.3.1.1 Assumptions 


Now that coordinate conventions have been established, the vehicle models 
can be explored. The first of these models is the bicycle model. In the bicycle 
model the inner and outer tires of each axle are represented by a single tire at 
the center of the vehicle’s axle. Thus the front is represented by a single front 
tire and the rear axle is represented by a single rear tire. So, the assumption is 
made that the inner and outer slip angles and the inner and outer steer angles 
are the same. The steering angle of the vehicle is represented by 6. The slip 
angle of the vehicle is denoted as B and describes the angle between the ve- 
locity vector and the longitudinal axis of the vehicle. The bicycle model also 
neglects weight transfer, the difference in vertical force between the inner and 
outer tires. 

Figure 3.3 represents the bicycle model. The CG is the center of grav- 
ity. The body fixed reference frame has an origin at the vehicle’s center of 
gravity. The z-axis is pointing down toward the ground, the x-axis is pointing 
toward the front of the vehicle, and the y-axis is pointing out to the vehicle’s 
passenger side. In Figure 3.3, dis the steering angle, and a and 6 are distances 


Figure 3.3. Bicycle model. 
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from the vehicle center of gravity to the front and rear axles, respectively. This 
model assumes that left and right steer angles are the same for front and rear 
axles. 


3.3.1.2 Kinematic Version 


At low speeds with no slip or minimal slip, the slip angles at the tires can 
be assumed to be zero, which produces the lateral kinematic version of the 
bicycle model. The perpendicular line from each tire passes through the same 
point, which is called the center of the turn. Figure 3.4 represents the kine- 
matic bicycle model. Note the perpendicular lines that originate at each axle 
and pass through the center of the radius, R. If the steering angle of the front 
tire reaches zero, the radius of curvature, R, goes to infinity. When the steer- 
ing angle is not zero the steering angle can be described by (3.2), known as 
the Ackerman angle. 


6 =tan™ o> = (3.2) 


By using simple kinematics, the vehicle’s velocity, V, can be described 
as the yaw rate, 7, of the vehicle times the radius of curvature, R. Also with no 
lateral sliding, the lateral acceleration, a,, of the vehicle is simply the centrip- 
etal acceleration developed during the turn. 


V =Rr (3.3) 
; V 
ay =V, =Rr? reel (3.4) 


Substituting (3.3) into (3.2) results in the expected yaw rate of the ki- 
nematic model, given a steer input and velocity. 


r =F dean(d) » 25 (3.5) 


3.3.1.3 Lateral Version 


For higher speeds that develop greater slip angles the lateral bicycle model can 
be used for vehicles that produce minimal roll angles. Figure 3.5 represents 
the lateral bicycle model where r is the yaw rate, V= [V, Y,] ris the velocity 
vector acting at the vehicle’s center of gravity, 6 represents the sideslip angle, 
d is the steering angle, and a and 6 are distances from the vehicle center of 
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Figure 3.4 Kinematic bicycle model. 


gravity to the front and rear axles, respectively. The front and rear tire slip 
angles are denoted as a, and a, respectively. By summing the forces and mo- 
ments about the CG on the free body diagram, Figure 3.5, and accounting 
for bank angle a simple set of dynamic equations can be derived to describe 
the vehicle’s lateral motion. Note that the force due to the bank angle will be 
the mass times gravity times the sin of the bank angle. 


a Fy =my =mV.B tV <r) =Fyr +Fyp +mgsin(Osank) (3.6) 


a M HL, =1.7 =aF yp +bF yp (3.7) 
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Figure 3.5 —_ Lateral bicycle model. 


Solving (3.6) and (3.7) for sideslip rate B and yaw acceleration 7 yields 


_ for cos(6) +Fyr +mgsin(Obank) oy 


B mV, 


(3.8) 


‘ _aF yp cos(6)- bF yr 
I, 


(3,9) 


To describe the front and rear tire forces, F,z.,, a linear tire model will 
be used. This model assumes the tire forces remain in the linear region of 
the tire and are proportional to the tire’s respective slip angle times the tire’s 
cornering stiffness, Cy, shown in (3.10) and (3.11). Tire models while be 
discussed more in depth in Section 3.4.3. 


Fp =- Cap Op (3.10) 


FyR == Coy Oy (3.11) 


Substituting (3.10) and (3.11) into (3.6) and (3.7), a state space repre- 
sentation of this model can be developed and is shown by (3.12). 


é Co 73 5 Ci u Caf U 
az, €© mV, ‘ 4a, € Uu 
a. y=e ioe fate.” US = (3.12) 
eu é a _ Oo: de uo Lore 
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where 


Co =Caf +Cor 
Cy =aCap - bCar (3.13) 
Co =a’Cag +b°Coy 


With a state space representation, this model can be configured for con- 
trol or estimation purposes. Control and estimation are discussed in proceed- 
ing chapters. 

Further examination of the lateral bicycle model can reveal the steady- 
state tire slip. Assuming yaw acceleration is equal to zero and lateral accelera- 
tion is equal to (3.4), the centripetal acceleration, then (3.6) and (3.7) are 
simplified. 


V2 
a SfyF +Fyr (3.14) 


0 =aF yp +bF yp (3.15) 


Substituting (3.10) and (3.11) into the above equations, the steady- 
state tire slip can be solved. 


WV 
~ Capek (3.16) 
a 
2 
a =o (3.17) 
ar & 


For control purposes it is advantageous to look at the relationship be- 
tween yaw rate, 7, and steer angle, 6, or between sideslip, B, and steer angle, 6. 
Combining (3.10) and (3.11) into (3.6) and (3.7) in an alternate manner and 
again neglecting the bank angle yields the state space representation: 


E- Co meh _ ul ECar U 
BU EnV, 2” “UBY Emv, U 
2 y= ee ig te” U6 (3.18) 
eq ea Co vert Sara 

& I, TV. 8 8&7, of 
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The equations for the transfer function from steer angle to yaw rate and 
from steer angle to sideslip are shown here. 


aCaf r 44 +b)CafCar 


AD. qd, mV,1 B) 
(s) 2 Col, +tmCr CoC? - CimV2 - Gy . 
so+ s+ : 
mV 1x mV 21, 
Cap. CapC2~ aCapi - mV, aCaf 
B(s) _ mV. mV, 1, (3 20) 
Os) 5 Cole tmC, =. CoCn - CimV2 - C? 
sot s+ 3 
mV 1 mV, 1, 


3.3.1.4 Longitudinal Dynamics 


The longitudinal dynamics of a vehicle are influenced by the longitudinal 
force of the tires. Tires cannot generate force without slipping. This will be 
discussed in greater detail in Section 3.4.2. In order for a vehicle to move 
longitudinally slip must occur at the wheels. The percent slip of a wheel is a 
function of the velocity, V, angular velocity, @, and effective radius, R,, of the 
wheel and is defined as 


- Re@ 


% slip = 
(3.21) 


The effective radius can be defined approximately as a function of the 
loaded radius, R,;, and the unloaded radius, Ry, of the wheel. 


ku - Ri 


R. » Ry ~ 3 


(3.22) 


Examining the x-z plane of the SAE coordinate system and using a half 
car model generates the free body diagram (Figure 3.6). The effect of drag is 
modeled as pV* acting on the CG where V is equivalent to x and the longi- 
tudinal velocity p is the drag coefficient. Note that wind speed is not taken 
into account. Also, note that the four wheels are combined into front and 
rear axles. 
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Figure 3.6 Longitudinal free body diagram. 


Neglecting air drag and summing the forces and moments yields the 
longitudinal equations of motion. 


AE, =mi =- Np - Np =mg (3.23) 
AE, =m =Fgp +E cp (3.24) 
OF Myce =1,0 =bN p - beg Eee +Fex)~ aN (3.25) 


Combining the equations of motion produces solutions for the normal 
forces acting on the front and rear axles. 


1,0 thom +amg - ami ; 


Mz 3.26 
atb ‘ ( 


Nr =mg - 


Al yO thegmi +amg - ami 


3.27 
atb ( 


R 


For the case of steady state pitch 6 = 0 and = g. The normal forces on 
the front and rear axles then become 


_ bmg _ mileg 
atb atb 


NF (3.28) 
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= — 2 
atb atb oe) 


Note that in the steady state pitch case the first term of Nand Np is the 
front and rear weight splits, respectively. 

For the case with air drag included, the summation of forces in the x- 
direction becomes 


AE, =mit =Ep +E. - px? (3.30) 


The effect of air drag is important when designing a velocity controller. 
The control gains can be linearized about the operating point. This will be 


discussed in Chapter 6. 


3.3.2 Understeer Gradient 


In order to develop a better understanding of the turning response of a vehicle, 
the understeer gradient of the vehicle is defined. Using the steady-state bicycle 
model, the understeer gradient can be determined from the weight distribu- 
tion and the cornering stiffness [8]. By including slip angles, a simple kine- 
matic equation between the steer angle and tire slip angles can be developed. 


5» = harp a, (3.31) 


Substituting (3.16) and (3.17) into (3.31) gives: 


L avy  W, laa fe 


6» 


+K ys Oy (3.32) 


From (3.32), the understeer gradient is labeled as Kys and is defined as: 


| BV wW,.0 
Kus =—.—"- 
& &Caf Car B 
(3.33) 
m BCor - atas® 0 
Kus = 
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The understeer gradient determines both the magnitude and the direc- 
tion of the steering inputs required for a given lateral acceleration [16]. The 
understeer gradient also determines if the vehicle is neutral steer, oversteer, 
or understeer. Figure 3.7 shows the basic principles with Ky; being the slope 
of each line. 


3.3.2.1 Neutral Steer 


Neutral steer occurs when the understeer gradient is zero, which results in 
the front and rear steady-state tire slip angles being equivalent. By studying 
(3.16), during neutral steer the steer angle required to make the turn is ap- 
proximately the Ackerman angle. 


3.3.2.2 Understeer 


Understeer occurs when Ky is greater than zero, causing larger slip angles to 
develop in the front tire than the rear. Because there is more slip at the front 
tire, the steer angle must increase to maintain the radius of the curve. During 
this condition, the steer angle increases linearly with the speed squared or the 
lateral acceleration. 


3.3.2.3 Oversteer 


Oversteer is the opposite of understeer. During oversteer, Kis is less than zero 
causing the rear tire slip angle to be greater than the front. Because the rear is 
sliding more than the front, less steer angle is required to navigate the turn. 


3.3.3 Four-Wheel Bicycle Model 


To cope with the effects of roll, roll models are used. This often requires 
modifying the bicycle model into a four-wheel bicycle model. The four-wheel 
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Steer angle (6) 


Oversteer 


Lateral acceleration (g) 


Figure 3.7 Basic understeer gradient plot. 
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bicycle model accounts for the difference between the inner and outer vertical 
tire forces. This difference is the weight transfer. Weight transfer is important 
to understand because the vertical force on tire affects the lateral and longi- 
tudinal forces of the tire that dictate the performance of handling dynamics 
and characteristics. Before exploring roll models, tire models and properties 
will be discussed. 

The dynamic equations of motion for a ground vehicle model are de- 
rived for a vehicle that possesses both front and rear steering capabilities as 
well as individual torque control of each wheel. This general model is easily 
adapted to simpler vehicles (e.g., vehicles with only front-wheel steering), by 
the assignment of a constant control input of zero to the appropriate (unavail- 
able) control effectors. The free body diagram for the vehicle under consid- 
eration is shown in Figure 3.8. A right-handed coordinate system is used for 
the derivation of the equations of motion. The body fixed reference frame has 
an origin at the vehicle’s center of gravity. The z-axis is pointing down toward 
the ground, the x-axis is pointing toward the front of the vehicle, and the y- 
axis is pointing out to the vehicle’s passenger side. In Figure 3.8, 7 is the yaw 
rate, V= [xy] is the velocity vector acting at the vehicle’s center of gravity, B 
represents sideslip angle, 6 is the steering angle, ¢ is the track width, and a and 
b are distances from the vehicle center of gravity to the front and rear axles, 


Figure 3.8 Four-wheel bicycle model. 
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respectively. Subscripts f 7, R, L denote front, rear, right, and left sides of the 
vehicle, respectively. For clarity, only the left slip angles (4 and @,7) and the 
right steer angles (Og and 6,7) are shown on Figure 3.8. This model assumes 
that left and right steer angles are the same for front and rear axles. Note that 
all forces F and slip angles o are drawn in the positive direction such that the 
lateral force F= Cat where Cy represents tire cornering stiffness. 

The sum of the moments about the vehicle’s center of gravity is written as 


A Meg =1.W =a(Fyp_ cos Of +Fye cos Of - Fez sin Of - Fg sin df) 


- Lye sind ¢ - Fy sin bf - Fy cos bf +Fyr cos bf ) 
(3.34) 
- b (Fy cos6, +Fyp cos 6, +Fzz sind, +F,R sin d,) 


£ A : 
- “(F,,, sind, - Fr sind, - For cos6, typ cos 6,) 
2 J J 


where /, is the moment of inertia about the yaw axis. The solution of (3.34) 


for Wyields 
y =? =& (Fn +F pp) cos Of - (Byer +Fer)sin 5y) 


t 
"L(y +Fyp)sindy ~ ygu~ Fyn)cos6y) 


(3.35) 
-b (Enc +Fyx)cos 6, HFop +R) sin 5,) 


- (Fp ~ Pye) Siti, = Peer, > Pipe )COS 5, ie 


which describes the nonlinear yaw dynamics of the vehicle. 

The sideslip angle B is the angle between the vehicle’s actual velocity 
vector V and its longitudinal velocity component V,. Therefore, the vehicle 
fixed velocities and corresponding accelerations due to sideslip are 


V, =VcosB V, =VcosB- VBsinB 
—_ (3.36) 
V, =VsinB V, =VsinB +VBcosB 
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The effects of yaw rate are then included to give the complete expres- 
sions for acceleration in the vehicle fixed reference frame 


* =V cosB- VBsinB- rV sinB (3.37) 


7 =VsinB- VBcosB- rV cosB (3.38) 
The summation of the forces in the y-axis yields 


a F, =my =(Fyz +Fygr) cos Of + Bex +FypR)sin Of +H Fyrt +F,p) cos 6, 


+H Fort +Eer)sind, (3.39) 


Equation (3.38) is substituted into (3.39) and solved for B to obtain the 
equation of motion for sideslip 


B =[(FAyn +Fy¢r)cos of + Fez +F¢p)sin Of + Foor +F),p)cos 6, 


H Ft, +Ferr)sin 6; ) (mV cosB)- V tan B/V - r (3.40) 
3.4 Tires 
3.4.1. Basics 


The tire-road relationship is governed by the contact patch. With the excep- 
tion of aerodynamic forces, all external forces on the vehicle are developed at 
the tire’s contact patch. The tire serves three basic functions: 


1. It supports the vertical load, while cushioning against road shocks. 
2. It develops longitudinal forces for acceleration and braking. 


3. It develops lateral forces for cornering. 


3.4.2 Contact Patch and Slip 


Due to the elastic properties of the tire the contact patch deforms under 
loads. This deformation results in the slip angle o&. Experimental data shows 
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that the lateral force on a tire displays a nonlinear relationship with the slip 
angle “alpha.” However, there is a linear portion for small angles of a. The 
slope of this linear portion is called the cornering stiffness C,. Equations 
(3.10) and (3.11) can be derived from the linear portion of experimental data 
similar to the data shown in Figure 3.9. 

Figure 3.9 shows typical characteristics of a tire under lateral force gen- 
eration modeled by the Fiala tire model. More information on this model will 
be discussed later. As shown in the plot, the lateral tire force remains linear 
with slip angle, as modeled by (3.10) and (3.11), until the tire becomes satu- 
rated. This model relates peak tire force to the tire-road friction, U, times the 
normal force, F,, known as the peak tire force. 

An increase in the normal or vertical force of a tire will cause the rela- 
tionship between the lateral force and the slip angle to change. This increase 
will yield a higher peak tire force. Vertical forces on the tire are not only 
important for ride characteristics, but also help to describe the maximum 
longitudinal and lateral forces developed by the tire. As shown in Figure 3.10, 
the magnitude of lateral and longitudinal tire force cannot exceed the peak 
tire force. When the magnitude of tire force reaches this point, sliding oc- 
curs. By studying Figure 3.10, it is obvious that the available longitudinal 
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Figure 3.9 Generic tire curve. 
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Figure 3.10 Circle of friction for tire forces. 


drive and braking force decreases with an increase in lateral force. Because 
of this effect, both forces must be taken into account during combined lat- 
eral and longitudinal tire force generation to develop an accurate vehicle 
model. 


3.4.3 Tire Models 


Several researchers have developed models to describe the generated tire 
forces. One of the most well-known models, called the Magic Formula tire 
model, was developed by Pacejka [4, 5, 7]. This model is an empirical for- 
mula capable of calculating lateral and longitudinal tire forces. Alternatively, 
the two models used in this book are the Fiala and Dugoff tire model. 


3.4.3.1 Fiala Tire Model 


The Fiala tire model was originally developed to estimate lateral tire force 
generation only [2]. The model, however, was transformed to represent both 
lateral and longitudinal forces [3]. To accomplish this transformation lateral 
and longitudinal tire stiffness (Cy, C,) are assumed to be equal. This as- 
sumption though does not always hold true. The total slip o for this model 
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is simply the magnitude of the lateral and longitudinal slip (0,, 6,), shown 


by (3.41). 
oO =J0; +o2 (3.41) 


To calculate the total slip, the individual values for slip must be known. 
Both the longitudinal and lateral slips are found using (3.42) and (3.43). 


off Ow - Ve 
Ox =f" "* during acceleration 
Toff Ow 
(3.42) 
O., =—~— during braking 
Toff Ow 
Ve 
oO, = tan(a) (3.43) 
Toff Ow 


Both slip values can be determined with the use of sensors. Assuming 
a parabolic pressure distribution on the tire’s contact patch, (3.44) is used to 
describe the magnitude of force on the tire, using the Fiala tire model. 


UE, g00- 31300) +55 (300) aif o £0, 
(3.44) 
F =uF, if 0? On 
The variable, o,,, is the value of total slip where sliding occurs in the 
Fiala tire model. As described by the circle of friction, sliding is assumed to 


begin when the maximum tire force is equal to UF, 


0 Caio 


(3.45) 


m 


The individual values of lateral and longitudinal tire force (F,, F,) can 
be obtained by breaking up the force magnitude F,. This is done by multiply- 
ing the force magnitude by the ratio of total slip to each forces respective slip, 


as shown in (3.46) and (3.47). 


FE, =—F (3.46) 


78 GNSS for Vehicle Control 


F,=—"F (3.47) 


In the case of pure lateral slip, set 0, = tan(@) and o, = 0 in the Fiala 
tire model. In case of pure longitudinal slip, set 0, = 0 [10]. By reducing 
the combined force generation model to either lateral or longitudinal force 
generation, simpler calculations can be performed if the amount of noisy 
measurements is reduced. 


3.4.3.2 Dugoff Tire Model 


The Dugoff model and Fiala model both allow for tire force estimates during 
combined lateral and longitudinal tire force generation. However, the Du- 
goff tire model assumes a uniform vertical pressure distribution on the tire’s 
contact patch [6]. This is a simplification from the Fiala’s tire model, but it 
allows for individual values of lateral and longitudinal tire stiffness. The lon- 
gitudinal and lateral tire forces are given by (3.48) and (3.49), respectively. 


F. =Cy (3.48) 
_ Ca tan(o) 
'y eas coroners iC. (3.49) 
where 
= uF (1 +0.) 3.50) 
; 
2@CoOx)” Hes tan(ar))? bP 
1(2- A)A ifA<l 
fA) = ee (3.51) 


1 ifA31 


Similar to the Fiala tire model, this model has a transition that occurs 
when A = 1. This transition occurs when the tire leaves the linear region and 
begins the nonlinear region. If the tire is experiencing lateral slip only, the 
model may be reduced by setting 6, = 0 or for pure longitudinal force genera- 
tion simply set @ = 0. This helps to simplify the model during driving condi- 
tions where only lateral or longitudinal forces are generated. 
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3.5 Roll Model 


It is very important to understand vehicle roll and rollover. Many research- 
ers have developed models to describe the roll dynamics of vehicles during 
cornering. Some models are fairly simple while others are very in depth and 
require more parameters. The less complex roll models do not include the 
springs and dampers of the suspension and therefore assume the sprung mass 
is stationary with the axle. Other high-fidelity models take into account forces 
produced by the springs and dampers. 


3.5.1 Free Body Diagram 


In order to produce a reliable roll model, a free body diagram (FBD) must be 
developed. The FBD in Figure 3.11 shows a two-state roll plane model [13, 14]. 
Three important parameters used in this model include the CG height, 4, 
roll stiffness, Ky, and roll damping coefficient, Cy. This model lumps the en- 
tire vehicle mass into the sprung mass. This assumption allows a simplified 
equation for the spring and damper torques, shown in (3.52) and (3.53). 


T spring =K oo (3.52) 
T damper =Ce (3.53) 


Notice that both equations also assume the spring and damper torques 


are linear with roll, @, and roll rate, @, respectively. Also, note that @ is equiv- 
alent to p, the SAE defined roll rate. 


| t 


Figure 3.11 Vehicle roll free body diagram. 


80 GNSS for Vehicle Control 


3.5.2 Equation of Motion 


By summing the moments about the roll center on Figure 3.11, a simple 
equation is derived to describe the roll dynamics of the vehicle. Equation 
(3.54) assumes the vehicle’s sprung mass rotates about a fixed point at the 
centerline of the lateral axis on the ground. 


Teg +Cy +K oo =hg (ay cos() +g sin()) (3.54) 


By assuming a steady-state turn and small angles, (3.54) can be sim- 
plified to solve for the roll angle with knowledge of the CG height and the 


spring roll stiffness. 


Mggty mregV* 3.55) 


@ = = 
Ko = Meg & RKo . mhrg g) 
3.5.3 State Space Representation 


Equation (3.55) may be transformed into a state space representation. The 
state space representation is shown in (3.56). 


au, 0 I Ug eo? 

QU _& Ko- mghe — Co GPU, Ghee (3.56) 
G.U=e Ip ee UY | 
ure) é Jef Jef GP a/f 4 


Many other models have also been used to analyze roll dynamics. Some 
models developed do not assume the vehicle’s roll center is located at ground 
height. One model developed in [11, 12] assumes the roll center is not at 
ground level and that the imaginary roll center derived produces reactionary 
forces. 


3.6 Additional Models Used in this Work 


In addition to the bicycle models two other models are used in later chapters 
to describe two-wheeled vehicles and trailer dynamics. 
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3.6.1 Two-Wheeled Vehicle 


The two-wheeled vehicle motion is modeled as a kinematic relationship to 
describe the position changes as shown in Figure 3.12. 

The variable 0 = [0,, Og] represents angular positions of the robot 
wheels. The variable d is the distance between the driving wheels, or track 
width. The vehicle’s change in position in the East, North, Up (ENU) Carte- 
sian coordinate frame can be found from the following kinematic equations. 


wW =r (3.57) 
é =V,,siny (3.58) 
n =V,, cosy (3.59) 
where 
Pex 3 
: = (6: - bp) (3.61) 


Figure 3.12 Two-wheeled vehicle kinematic diagram. 
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A state space representation of the transform from body fixed XYZ 
frame to the global ENU frame is shown in (3.62). 


3.6.2 Trailer Model 


gu éinyd 
a i = Kos Tae + 4 (3.62) 
eeeok ef 


A kinematic model is used to describe a trailer or implement towed behind 
the vehicle. A schematic of a vehicle pulling a trailer is shown in Figure 3.13. 
The trailer’s point of zero lateral velocity, ZLV, as well as its control point, 


Figure 3.13 Vehicle-trailer free body diagram. 
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cp, are shown in Figure 3.13. The ZLV is the point on the implement where 
the lateral velocity is equal to zero. For a two-wheeled trailer, the ZLV point 
is assumed to be the point at the center of the axle between the two wheels. 
For farm implements or more complicated trailer designs, more sophisticated 
system identification techniques may be required to determine the ZLV 
point. The control point defines the point on the trailer that should follow 
the specified path. This point can be chosen by the designer and is often 
chosen to be the location of the GPS antenna on the trailer if one is used to 
determine the trailer’s position. 

A kinematic model that relates the vehicle velocity, V,, and yaw rate, 
r, to the angle between the vehicle and the trailer is referred to as the hitch 
angle, @, and is given by (3.63) 


~ =- rg tT cos Oa: : sin (3.63) 


where L;, is the distance from the CG of the vehicle to the hitch point and 
Lr is the distance from the hitch point to the ZLV point of the trailer. The 
model is derived by looking at how vehicle velocity and yaw rate translate 
into velocities at the tow pin. The resulting tow pin velocities can then be 
used to determine the rotational velocity of the implement assuming zero 
lateral velocity at the ZLV point. 


Table 3.1 

Properties of Carsim’s G35 Sedan 
Wheelbase L 2.85m 
Vehicle Mass m 940 kg 
Dist. from CG to Front Contact Patch a 1.019m 
Dist. from CG to Rear Contact Patch Db 1.831m 
Yaw Moment of Inertia L 1,530 kg * m? 
Front Tire Cornering Stiffness Cot 78,311 N/rad 
Rear Tire Cornering Stiffness C3 47,033 N/rad 
Injected Noise Value on a, GE, (.1 m/s2)? 
Injected Noise Value on r of (.02 rad/s)? 
Injected Noise Value on B oj (.02 rad)? 
Injected Noise Value on V o (.01 m/s)? 
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3.7 Vehicle Model Validation 


To show the accuracy and limitations of the models, each model is validated 
with experimental data. The data is from a G35 sedan at the National Center 
for Asphalt Technology (NCAT) test track. The Infiniti G35 sedan is imple- 
mented with a dual antenna GPS unit, wheel speed sensor, a 6 DOF inertial 
measurement unit (IMU), and an optical encoder for steer angle measure- 
ments. Carsim, a high-fidelity vehicle simulation software, is also used for 
roll model verification. 

With the data gathered at the NCAT test track, the kinematic and bi- 
cycle model are validated in MATLAB. The parameter values used in the 
simulations of the G35 sedan are listed in Table 3.1. 

By using MATLAB to simulate the dynamic equations presented in 
this chapter, Figure 3.14 shows that both the kinematic and bicycle model 
matches the recorded data at 2 m/s, as would be expected. However, for larger 
slip angles, the assumptions of the kinematic model break down causing the 
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Figure 3.14 Comparison of the kinematic and bicycle model during slow-speed turning in 
the G35 sedan. 
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model to perform poorly. In the data logged at NCAT, slip angles remained 
small enough for both the bicycle and kinematic model to hold true. When 
vehicles reach higher speeds, as shown in the next experiment, the simplistic 
kinematic model is not the best choice. 

To show the shortcomings of the kinematic model, data was logged 
in a G35 sedan at higher speeds around the NCAT test track. The inputs 
were run through both models in MATLAB and the results are shown in 
Figure 3.15. Notice the difference in the kinematic and bicycle models pre- 
diction of yaw rate. While cornering at high speeds with large slip angles, the 
kinematic model cannot accurately predict the vehicle’s dynamics. 

To illustrate the shortcomings of the bicycle model with a linear tire 
model, a maneuver is conducted that saturates the tires enough for the vehicle 
to begin sliding. This is a very hard maneuver and is conducted to show that 
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Figure 3.15 Comparison of the kinematic and bicycle model during high-speed cornering 
in the G35 sedan. 
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Figure 3.16 Comparison of the bicycle model with linear and nonlinear tire models during 
high-speed sliding experiments in the G35 sedan. 


a linear tire model without saturation cannot describe the vehicle motion at 
the limits of handling. The more advanced model uses the Dugoff tire model 
to calculate the lateral tire forces. Figure 3.16 shows the bicycle model with 
a linear tire failing to match the data when the vehicle loses control. How- 
ever, the bicycle model with a nonlinear tire model matches the data fairly 
well, although there is still some mismatch at the highest peak. This is most 
likely due to the fact that the model does not take into account vehicle roll 
dynamics. 

Finally, the two-state roll plane model is tested. The vehicle used in 
this simulation is a large SUV from Carsim. Carsim is a high-fidelity vehicle 
simulation tool that can be used to validate simplified vehicle models. Carsim 
is chosen in this experiment because a vehicle is needed that produces large roll 
angles, unlike the G35 sedan used in previous experiments. Table 3.2 provides 
the parameter values used in the roll plane model during this simulation. 
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2,450 kg 
1.1m 
1,243 kg * m2 


25o7gN_m 
de 


Nom 
152.05 
deg 
1.62m 
(.1 m/s?) 
(.01 rad)? 


(.005 rad/s)? 


Table 3.2 
Properties of Carsim’s Large SUV 

Vehicle Mass m 
CG Height (Sprung Mass) heg 
Roll Mass Moment of Inertia Jott 
Roll Stiffness Ky 
Roll Damping Cs 
Track Width T 
Injected Noise Value on a, 03, 
Injected Noise Value on @ oS 
Injected Noise Value on @ oO; 


S 


The large SUV attempts a double-lane change in order to induce large 
roll rates and angles. The data from Carsim is used to compare with the 
simple two-state roll plane model, which is simulated in MATLAB. The 
data from Carsim matches up well with the simple roll plane model, given 
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Roll plane model validation of a double lane change maneuver in Carsim. 
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in (3.55), as shown in Figure 3.17. One difference in this model and the 
Carsim high-fidelity model is the springs used in the Carsim model are 
highly nonlinear. Shown by (3.54), the simple roll plane model assumes 
linear springs [9]. 


You should now have an adequate understanding and introduction to 


the vehicle models used throughout the proceeding chapters. 


[13 
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Navigation Systems 
William Travis 


4.1 Introduction 


The demand for accurate and reliable navigation for ground vehicles is pro- 
liferating as passenger vehicle control systems become more complex and as 
the market for autonomous capability continues to grow. In addition to ve- 
hicle state information, a reliance on a navigation solution will be required 
as vehicle stability control systems increase their intelligence beyond the cur- 
rent versions of electronic stability control (ESC), vehicle dynamic control 
(VDC), and so forth. Future intelligent and safety vehicle systems may use 
this information to provide better lane keeping capability, higher accuracy 
vehicle tracking, or enhanced driver assistance systems. 

Control of unmanned ground vehicles (UGV) requires precise navi- 
gation information. The need for reliable navigation information will only 
increase as passenger vehicles continue to acquire UGV-like capabilities and 
as UGVs become more common in both military and civilian markets. An 
increase in the precision of the navigation solution can directly lead to a safer, 
more capable, and more robust autonomous system. 

The goal of this chapter is to provide GPS- and IMU-based navigation 
algorithms capable of providing the basic information necessary to autono- 
mously control a ground vehicle. Such information can include, and is not 
limited to, ground speed, direction of travel, position, orientation, and velocity 
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components. The majority of the algorithms can be implemented using the 
standard NMEA RMC output from any GPS receiver. An RMC message 
contains the essential navigation data of time, speed over ground, course, 
latitude, and longitude. A standard IMU provides accelerations along and ro- 
tational rates about a three-axis coordinate system, but the algorithms shown 
will use a reduced IMU. 

The following sections discuss several algorithms and their limitations 
when used on a ground vehicle. The discussion starts with a brief description 
of a Kalman filter, which is the base algorithm used in navigation state esti- 
mation. Two GPS/INS integration architectures are then discussed: loose and 
close coupling. The differences, advantages, and hindrances are presented. 
Two simple speed estimation algorithms follow; one incorporates accelera- 
tions and GPS speed measurements, and the other adds a wheel speed mea- 
surement while accounting for longitudinal wheel slip. A heading estimation 
algorithm uses a yaw rate gyroscope in conjunction with the RMC course 
measurement. A full, loosely coupled navigation algorithm is described that 
produces estimates of vehicle speed, heading, and position using a reduced 
IMU, an odometer, and data in the RMC message. A brief discussion of mod- 
eling deficiencies in the presence of vehicle sideslip urges the system designer 
to know the limitations of the designed system. An algorithm incorporating 
a dual antenna GPS system capable of estimating lateral vehicle motion is 
described. Finally, a closely coupled algorithm is described that incorporates 
a full IMU and GPS range measurements to produce three dimensional posi- 
tion, velocity, and attitude states. 

Readers interested in learning more about navigation systems and their 
various applications are referred to [1-7]. Specifically, [1-3] focus on multi- 
sensor fusion for navigation applications, and [3] provides a detailed overview 
of sensor operation and error sources. GPS is the main focus in [4-7], where 
the GPS system, receiver operation, and error sources are described in detail. 


4.2 Kalman Filter 


The Kalman filter is an unbiased, minimum variance estimator that is the 
backbone of many navigation systems. The use of this estimator is a proven 
method for blending measurements to minimize various sensor deficiencies 
while utilizing the strengths of each sensor by statistically weighting each 
measurement. For instance, an integrated accelerometer can provide a high- 
rate velocity measurement, but with poor short-term stability. A GPS-based 
velocity measurement has a low output rate, but excellent long-term stabil- 
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Figure 4.1. An integrated accelerometer (gray) provides a high-rate speed measurement 
(50 Hz), but quickly drifts off of truth. GPS provides a drift-free speed measure- 
ment (dotted line), but at a low rate (5 Hz). A Kalman filter was used to calculate 
a high-rate, drift-free speed estimate (black). 


ity. The two measurements can be combined in a Kalman filter to produce 
a stable, high rate velocity estimate. This scenario is depicted in Figure 4.1, 
where the integrated accelerometer quickly drifts from the true value, but the 
Kalman filter output is a high rate and drift free estimate of speed. 

A derivation of least squares methods and the Kalman filter are pro- 
vided in the Appendix. If more detail is desired, consult the references given 
in the Appendix. 


4.3 GPS/INS Integration Architectures 


Two integration architectures are discussed in this chapter. More options 
exist [4], but for practical purposes and ease of implementation the discus- 
sion is limited to loosely coupled and closely coupled GPS/INS integration 
methods. 


4.3.1 Loose Coupling 


A loosely coupled GPS/INS integration routine combines the inertial mea- 
surements with position and velocity measurements calculated by the GPS 
receiver. Feedback to the inertial processor calibrates the IMU to remove 
effects from biases, scale factors, and/or misalignment. 
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Some benefits to adopting this strategy: 


The measurement vector remains a small and constant size because 
measurements are position and velocity from the GPS receiver. This 
reduces some complexity in implementation, and the computational 
burden is low due to the relatively small size of the measurement vec- 
tor and corresponding measurement covariance matrix. 


The routine is easily adaptable to partial IMU integration. A full 
IMU is not necessary for some applications, so the designer can re- 
duce both financial and computational cost by using the necessary 
number of accelerometers and/or rate gyroscopes. 


Some drawbacks of this strategy are that: 


The measurement quality is dependent upon the GPS receiver. The 
GPS navigation solution can contain large jumps when a satellite 
comes in or out of view or when multipath is present in the signal. 
The navigation designer has limited options available to mitigate 
these effects. 


A minimum of four GPS satellites must be in view to solve for the X, 
Y, Z, and time components of a position solution. A solution might 
not be provided when fewer than four satellites are visible, and infor- 
mation from the visible satellites is not utilized. 


4.3.2 Close Coupling 


A closely coupled GPS/INS integration routine combines the inertial mea- 
surements with range information to the GPS satellites provided by the GPS 
receiver. Feedback to the inertial processor is retained, but there is no feedback 
to the GPS receiver (a tightly coupled architecture uses the navigation solution 
to aid the GPS receiver’s tracking loops, but a specialized receiver is necessary 
and therefore is not further discussed). Range information includes a user to 
satellite distance and user to satellite velocity. Range information is given by 
the pseudorange or carrier phase measurement, and the range rate information 
is a function of the Doppler frequency or carrier phase measurement. 
Benefits of a close coupling architecture are that: 


The range measurements are processed individually, so the user has 
more influence on their impact on the navigation solution. The 
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measurements can be individually weighted, so suspected errone- 
ous measurements can be deweighted or thrown out altogether. This 
leads to a smoother solution, and when combined with inertial data, 
a solution that is much less apt to contain large deviations. 


The navigation system can utilize range information when three or 
fewer satellites are visible. A GPS receiver might not provide posi- 
tion and velocity data in such a scenario, but the user position is still 
partially constrained by the available range information. 


Shortcomings of this method: 


The measurement vector will fluctuate in size as satellites come in 
and out of view, so appropriate logical steps need to be installed. 


¢ The measurement vector can be relatively large, which increases the 
computational requirements. If 12 satellites are visible, then 12 range 
and 12 range rate measurements will be available; the measurement 
vector will be 24 x 1 and measurement covariance matrix will be 
24 x 24. The user can, however, reduce the number of measurements 
by using highest quality information that places the best geometrical 
constraint on the solution. 


4.4 Speed Estimation 


An accurate measurement of vehicle speed is required to control both the 
vehicle’s speed and position. A good controller requires a quality measure- 
ment at a sufficient rate. A designer can have several options when it comes 
to measuring vehicle speed, but the following discussion will focus on three 
specific sensors used to obtain a speed measurement: GPS, an accelerometer, 
and an odometer. 

Integration of an onboard accelerometer can provide a high update rate 
speed measurement, provided an initial speed is well known. Unfortunately, 
integration of the errors present in the accelerometer output, such as scale 
factor, bias, and noise, will corrupt the measurement. Often the scale factor 
can be calibrated, but the accelerometer bias is time varying. The velocity 
measurement will drift off if the accelerometer is integrated of the over a 
period of time. 

Even the cheapest GPS receivers can provide a quality speed measure- 
ment with an error standard deviation of less than 5 cm/s. High-cost receivers 
can provide slightly improved accuracy and update rates of 10, 20, or even 
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50 Hz. However, the output rate of a low cost receiver is usually between 
1 to 5 Hz, and can be insufficient for control purposes or for other sub- 
systems that might rely on the navigation solution. 

A mathematical model of the system must be derived in order to imple- 
ment a navigation system. Typically, there are two types of models: param- 
eter based and kinematic. A parameter based model is a set of differential 
equations using physical information about the system. For a parameter 
based navigation system model of a UGV, the UGV’s mass, wheelbase, track 
width, weight split, and tire cornering stiffness would be incorporated into 
the navigation system. The shortcoming to this type of model is the lack of 
certainty and variation in some model parameters. A kinematic model relies 
solely on kinematic relationships between the sensors and is independent of 
physical parameters. This improves the robustness of the navigation system, 
but sacrifices motion constraints of the vehicle. 


4.4.1 Accelerometer and GPS 


The first estimator example combines a GPS speed measurement with an 
integrated accelerometer measurement. A simple model of an accelerometer 
output contains the time rate of change of speed, v, a moving bias, b, and zero 
mean, white noise, @,(@, N(002)). 


a=0+64+0, (4.1) 


The key assumptions of the above model are the vehicle is traveling in a 
straight line on level ground and the accelerometer is located at the vehicle’s 
center of gravity. Effects due to road grade, vehicle pitch, and centripetal 
acceleration are neglected. From this equation, a dynamic model for the ve- 
hicle’s velocity can be constructed. 


v=a, b WM (4.2) 


The bias can be modeled as integrated zero mean, white noise (@, 
N(0,0;)) to capture its motion. 


b =a, (4.3) 


A continuous state space representation of the dynamic model is shown 
here. 
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The state vector is defined as x = [v 6]’, the input z is the accelerometer 
output, and the process noise vector is defined as @ = [@, @,]’. The continu- 
ous process noise matrix for this system is calculated by evaluating (A.3). 


t 
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Initial values for 0, and oj are often listed in the sensor specification 
sheet that is provided by the manufacturer. These values can be altered, or 
“hand tuned,” to adjust estimator performance for custom applications. Of- 
ten, the accelerometer will record unwanted vibrations in the system, but the 
value for 6, can be inflated to account for the additional process noise. 

The above system must be discretized for use in a discrete linear Kal- 
man filter. The sample rate, Ds, is chosen based on the update rate of the 
input, which is the accelerometer in this instance. This will also be the rate 
at which estimates of the state vector are available. If no input is used in a 
Kalman filter, the sample rate can be chosen at the discretion of the system 
designer. The discrete state matrices and the discrete process noise matrix for 
this system are as follows: 


4 ce 
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TROON 


The GPS speed measurement is modeled at zero mean, white noise (V, ~ 
N(0,02) added to the true speed. 
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Z=v+v (4.9) 


The measurement matrix varies as a GPS measurement is received. 
When a measurement is available, the update can be calculated with the fol- 
lowing matrix: 


H =(1 0] (4.10) 


When a GPS measurement is not available, the measurement matrix is 
simply a row of zeros. Alternatively, the Kalman gain calculation and mea- 
surement update can be skipped. 

Next, the measurement covariance matrix must be constructed. The 
measurement covariance matrix is clear-cut for this example as there is only 
one measurement available at each epoch. Therefore, it is equal to the vari- 
ance of the noise on the GPS speed measurement. 


R=657 (4.11) 


A GPS speed measurement has a standard deviation of below 5 cm/s. 
Even the lowest-cost receivers can perform at this level. Some higher-end 
receivers have quoted performance levels down to 1 cm/s. 

Since the measurement in this application is inherently discrete, there is 
no need for discretization. Equations (4.6) through (4.8), (4.10), and (4.11) 
can be used ina linear discrete Kalman filter to estimate speed and accelerom- 
eter bias given measurements of acceleration and GPS speed. 
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Figure 4.2 A 50-Hz estimated speed is indicated by the line, and a 5-Hz GPS speed mea- 
surement is indicated by the dots. The accuracy of the estimated speed is 
superior to that of the GPS speed. 
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Figure 4.3 The speed measurement residual (dots) appears to be a zero mean Gaussian 
signal. 


The following figures display the estimator output when simulated ac- 
celerometer and GPS speed measurements were run through a Kalman filter. 
Figure 4.2 shows the estimated speed and GPS speed. The true speed during 
the simulation was a constant 10 m/s. The Kalman filter produced an esti- 
mated that accurately tracked the true speed. The estimated speed is output 
at the same rate as the accelerometer (50 Hz), but without the drift. Also, the 
variance of the estimated speed is slightly lower than that of the GPS speed 
measurement. 

Figure 4.3 displays the measurement residual for the speed state. The 
error appears to be zero mean white noise; no noticeable frequency or biases 
are present. The residual is bounded by the estimated standard deviation of 
the speed state, where Ox, =,/Pi. 

Closer examination of the error bounds reveals a saw tooth shape, as 
depicted in Figure 4.4. This shape is created by the inflation of the estimated 
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Figure 4.4 The estimated state covariance increases when GPS measurements are not 
available, but decreases when a new measurement arrives. This creates a saw 
tooth pattern in the error bounds as the estimator is propagated at a higher rate 
than which measurements arrive. 
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Figure 4.5 The estimated sensor bias (black) is compared to the simulated sensor bias 
(gray). 


state covariance when a GPS measurement is unavailable. The accelerometer 
is used to propagate the estimate at a 50-Hz rate, but GPS measurements are 
only available at 5 Hz. Propagation of a state through time perpetuates a loss 
in confidence of the accuracy of the state. A measurement provides a means 
of verifying and updating the estimate, and therefore confidence is regained. 
Therefore in nine out of ten iterations, confidence in the measurement slowly 
decreases as the accelerometer is integrated. Confidence is regained after the 
estimate is updated with a new GPS measurement. 

The estimated bias is shown in Figure 4.5. Since the data was simu- 
lated, truth is known. The bias estimate takes approximately 5 seconds to 
converge to the true value. The estimated bias accurately tracks the true bias, 
even as it slowly changes with time. 

In practice, a visual confirmation of a correct bias estimate is not avail- 
able. The designer must resort to other means to verify that the bias estimate 
is indeed correct. If the bias estimate is incorrect, the error will manifest itself 
in other states. The measurement residuals should be checked to ensure their 


Bias (m/s?) 


10 12 14 16 18 20 
Time (s) 


Figure 4.6 The bias estimate is constant during the GPS outage from 15 to 20 seconds. 
During this time, it is unobservable. 
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Figure 4.7 The speed estimate exhibits slight drift during the short, 5-second GPS outage. 


content is Gaussian. Analysis of the measurement obtained by integrating the 
accelerometer with instantaneous values of the bias estimate subtracted out 
also indicates the quality of the bias estimate. Artificial removal of the GPS 
measurement can also validate the bias estimate. If the bias estimate is correct, 
the speed estimate will exhibit minimal drift through the outage. An incor- 
rect bias estimate will cause the speed estimate error to grow quickly. 

The covariance matrices should be adjusted if poor performance is de- 
termined. Values of a sensor’s drift rate provided by the manufacturer are a 
good starting point when tuning the Kalman filter. These values should be 
altered until the residuals are white and acceptable performance is achieved 
when GPS is removed for short periods of time. 

The GPS speed measurement was artificially removed between 15 and 
20 seconds to demonstrate the estimator’s dead reckoning performance. The 
accelerometer bias is unobservable during the GPS outage, and its estimate 
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Figure 4.8 The variance of the speed estimate error increases when GPS is unavailable to 
indicate a loss of certainty in the estimate. 
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is predicted only with the dynamic model. Figure 4.6 plots the bias estimate. 
Notice the estimate remains constant during the GPS outage. The Kalman 
filter is unable to predict any variation in the accelerometer bias. The bias es- 
timate remains reasonably accurate because of the minimal variation over the 
period of the outage. However, the bias estimate error could get quite large 
during an extended outage and corrupt other states in the filter. 

The accuracy of the speed estimate during the outage is acceptable for 
many applications. Slight drift is noticeable in Figure 4.7 and is due to the 
small error in the bias estimate. 

As expected, the estimated error variance grows during the GPS outage 
due to the integration of sensor noise during the time update. The error stan- 
dard deviation is approximately 5 cm/s at the start of the outage but quickly 
grows to 25 cm/s in 5 seconds, as seen in Figure 4.8. 

A trick the navigation system designer can implement to assist the 
filter is to input a zero velocity measurement with a low variance when 
the vehicle is stationary. The zero velocity provides a means to accurately 
estimate the sensor bias as measurement noise has been eliminated. There 
must be high confidence that the vehicle is indeed not moving before the 
speed measurement is set; otherwise small motions cannot be captured by 
the estimator. The designer should be careful not to divide by velocity as 
well. 


4.4.2 Accelerometer, GPS, and Wheel Speed 


The output of an odometer, such as rotary encoders or Hall effect sensors, 
can be differentiated to provide a speed measurement. The measurement up- 
date rate is typically sufficient for navigation, but the output can be noisy 
due to the differentiation of a quantized signal. Also, changes in wheel radius 
or longitudinal wheel slip can corrupt the speed measurement. These effects 
could be neglected (with caution) on a passenger vehicle in a benign highway 
environment in good weather. However, off-road applications are more con- 
ducive to the generation of wheel slip and radius changes due to terrain and 
typically softer tires. 

The following example constructs a navigation system using measure- 
ments of acceleration, GPS speed, and wheel speed. The assumptions of the 
previous model are still in place. The wheel radius will be assumed to be 
constant, but tire slip will be anticipated. Therefore, the navigation system 
will be designed to handle intermittent slippage and estimate the amount of 
slip occurring. 
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Longitudinal tire slip is expressed as a percentage, and defined as the 
difference between the wheel velocity and true velocity divided by the true 
velocity. 

_ Oy - v 
su 


(4.12) 
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The true velocity is denoted by v, the wheel radius is 7, the rotational 
wheel speed is @,,, and the percentage of longitudinal slip is s. Simple ma- 
nipulation of the equation yields and expression for the output of a wheel 
speed sensor. 


Uws =76,,, =(1 +s)v (4.13) 


Knowledge of the slip dynamics must be known if the slip is to be 
estimated. Approximating the wheel slip as integrated white noise would be 
a simple starting point. A more complicated model, possibly incorporating 
applied torque, could be pursued if the simple approximation proves to be 
relatively successful. This example will focus on the simple case, and model 
the slip with zero mean white noise (@,=N (0,0 2) as follows: 


The previous example can now be incorporated. Assuming a state vec- 
tor of = [v 6 s]’, an accelerometer for an input, and a process noise vector of 
@ = [@, @, @]’, a continuous state space representation can be developed 
for the linear dynamic models. 
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The continuous process covariance matrix contains the independent 
variances along the diagonal. 
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Discretization yields the following: 
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The measurement vector is constructed using the same GPS speed 
model and the wheel speed model given by (4.9). 
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The measurement model for wheel speed contains two states multiplied 
together. This is a nonlinear model; therefore the extended Kalman filter 
must be used. A measurement matrix can be calculated by linearizing the 
model with respect to the state vector. 
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The measurement covariance contains the individual measurement 
variances along the diagonal with zeros in the off diagonal elements. 
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Figure 4.9 The wheel speed (gray), GPS speed (dots), and speed estimate (black) are 
depicted. 


The differentiated encoder noise can be approximated as Gaussian in 
many cases, especially when the encoder resolution is high. 

The simulation of the previous example was regenerated with the aug- 
mented matrices and additional wheel speed measurement. Figure 4.9 shows 
the speed estimate with the two speed measurements. The longitudinal slip 
causes the wheel speed measurement to deviate from the true speed. How- 
ever, the error does not propagate through to the speed estimate because the 
slip is also estimated. 

Figure 4.10 is a close-up view of Figure 4.9. The presence of slip is 
clear, but the magnitude is hard to discern because of the low resolution of 
the wheel speed measurement. 

The slip estimate tracks the true wheel slip, as depicted in Figure 4.11. 
Shown in the picture is the true wheel slip in gray, the estimated wheel slip in 
black, and a measurement of wheel slip in black dots. The measurement was 
constructed using the odometer and GPS, and therefore is only available at 
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Figure 4.10 A closer look reveals the quantized nature of the wheel speed measurement. 
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Figure 4.11 The actual slip is shown in gray, the estimated slip is shown in black, and a 
measured value of slip using the wheel speed and GPS is shown in dots. The 
estimator tracks the wheel slip adequately for many applications and is supe- 
rior to the slip measurement. The resolution of the slip measurement is poor 
and its update rate is slow. 


5 Hz. The resolution of the measurement is severely limited by the quantiza- 
tion of the wheel angle measured by the odometer. The slip estimate, on the 
other hand, has a much higher resolution and is available at 50 Hz. The ac- 
curacy of the estimate is suitable for some applications, but systems requiring 
less noise would have to configure a different model. 

The bias estimate, plotted in Figure 4.12 with the actual bias, is slightly 
improved by the availability of an additional 50-Hz speed measurement. If 
GPS is lost, the bias will still be observable provided the wheel does not slip. 
Any slip of the wheel will corrupt the wheel slip estimate and bias estimate 
because the two signals are indistinguishable without a GPS speed measure- 
ment. The error cannot be attributed to wheel slip or a change in bias with 
any certainty. 
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Figure 4.12 The bias estimate is slightly improved due to the additional 50-Hz speed 
measurement. 
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4.5 Heading Estimation 


The RMC message contains a course or track over ground measurement. 
Integration of a yaw rate gyro provides a measurement of vehicle heading, if 
properly initialized. An equivalency between these two measurements exists if 
and only if ground vehicle exhibits no lateral motion. Any amount of lateral 
motion will inject a discrepancy between these two measurements. Vehicle 
heading is the pointing direction of the vehicle, while vehicle course is the di- 
rection of travel. The difference between vehicle heading and course is known 
as the vehicle sideslip. 

Under the assumption of zero sideslip, the GPS RMC course measure- 
ment can be used to calibrate a yaw rate gyroscope and provide a high rate 
vehicle heading estimate. The estimation model is similar to the speed es- 
timation model. A yaw rate gyroscope provides a measurement of the true 
turning rate with an additive moving bias and zero mean, white noise. The 
true yaw rate and bias can be modeled as follows: 


W=r- by - We (4.23) 
by =O (4.24) 


In the above expressions, W is the true yaw rate of the vehicle, r is 
the measured yaw rate, 0, is the yaw rate gyroscope bias, ,(@,~V (0,62) 
is the yaw rate gyro noise, and @,(@,~1V(0,0%)) is the noise driving the bias 
drift. Assuming a state vector in the form x = [y 0] T’ process noise vector 
@ = [@, @,]", and input u = 7, the state space representation can be formu- 
lated like the first speed estimation example. 
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The measurement model for GPS course consists of the true heading 
plus zero mean white noise (v =N (0,04). This model assumes sideslip is 
zero. 


Wors =y +v (4.30) 


The noise amplitude of a GPS track over ground measurement is speed- 
dependent due to the nature by which it is generated. The course is based 
off the direction of travel; therefore, a course does not exist for a stationary 
object. The course is better defined as the speed of the object increases. The 
standard deviation of a GPS course measurement is inversely proportional to 
the speed over ground at the antenna, as seen in Figure 4.13. 

The accuracy metric is defined below, where 0, is the GPS speed 
accuracy. 
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Figure 4.13 GPS course accuracy is inversely proportional to the speed over ground. 
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Therefore, the vehicle’s speed must be known to compute the GPS 
course accuracy, and the measurement covariance must be updated each it- 
eration. In practice, a logical step based on a speed threshold can be imple- 
mented to prevent a divide-by-zero or unnecessarily large covariance. The 
measurement covariance consists of the only the GPS course variance (this 
example assumes that knowledge of speed is readily available, so only the 
heading and yaw gyro bias are estimated). 


42 ? 
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Integration of the rate gyroscope yields an unbounded measurement 
of heading. Heading measurements would increment by 27 after each 
complete revolution about some fixed point. The GPS course measure- 
ment, however, is bounded between 0 and 27. This discrepancy must be 
addressed to correctly determine the error between the two measurements. 
The designer can either bound the integrated rate measurement (and esti- 
mated state) or unbound the GPS course measurement, neither of which is 
overly tedious. 

Removing the bound on the GPS course measurement is a single step 
installed in the filter. An incremental counter tracks the number of complete 
clockwise or counterclockwise revolutions. The estimate can be bounded af- 
ter processing for analysis or use in other subsystems. 

Three steps must be added in the filter if the GPS course measure- 
ment is not unbounded, but the estimate is ready for immediate analysis 
or further processing. The estimate must be checked and altered after the 
time update and after the measurement update to keep it within 0 to 27 
or +7. The measurement residual must also be placed within the desired 
boundary. 

Real data was processed in a linear discrete Kalman filter to produce a 
vehicle heading estimate using the algorithm described above. The vehicle 
started from rest, drove in a counterclockwise manner, and returned to rest af- 
ter a single revolution. Figure 4.14 shows the heading estimate, the GPS course 
measurement, and the integrated yaw rate gyro output after initialization. 

The GPS course measurement is only noise at the beginning and end 
of the run because the vehicle is stationary. The heading estimate cannot be 


110 GNSS for Vehicle Control 
f Yaw Rate 
= GPS 
—— Estimate 

r)) 

om 

= 

no 

= 

ne} 

oOo 

oO 

x= 

0 50 100 150 
Time (s) 

Figure 4.14 The heading estimate (solid line) is compared to the GPS course measure- 


ment (dots) and an integrated yaw rate gyroscope (dashed line). 


properly initialized until the vehicle moves. The estimate drifts from the user 
initialized value due to the integration of sensor bias and noise. A similar situ- 
ation is encountered if the vehicle comes to rest. 

Caution must be taken if the estimator is allowed to run during long 
periods of stationary activity. The estimate error covariance will grow dur- 
ing successive iterations of the time propagation and can reach unreason- 
ably large values. A logical step can be installed to maintain the estimate 
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The heading accuracy estimate degrades during a GPS outage to account for 
the integration of noise and a drifting bias. 
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Figure 4.16 The heading estimate slowly drifts from the true value during the 20-second 
GPS outage. 


integrity during motionless periods if the heading estimate has been initial- 
ized (i.e., the vehicle has been in motion previously). The last accurate mea- 
surement or estimate can be fed into the Kalman filter as a measurement 
since the GPS course measurement is unreliable, and bias observability will 
be retained. 

The heading estimate is able to maintain some precision if GPS is lost 
if the bias is correctly estimated. GPS was artificially removed from 100 to 
120 seconds. The estimator integrated the yaw rate measurement with the es- 
timated bias removed during this time. The covariance inflates as confidence 
in the heading estimate is lost, as seen in Figure 4.15. Figure 4.16 shows the 
heading estimate during the outage. 


4.6 Position, Speed, and Heading Estimation 


The NMEA RMC message contains all measurements necessary for basic 
navigation: time, course, speed, and position. Previous sections discussed the 
integration of a partial IMU with components of the RMC message. This 
section will develop navigation system equations that combine a partial IMU 
with GPS course, speed, and position. The position measurement provides 
additional information about the vehicle’s velocity, so the overall navigation 
system robustness is improved. 
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Table 4.1 
WGS84 Parameters 
Equatorial radius a 6,378,137 meters 


Reciprocal flattening 1/f 298.257223563 


Eccentricity e Jf(2_ fy 


Normal radius Ry Ay =, 


4.6.1 Coordinate Conversion 


The units of latitude and longitude, as output from an NMEA RMC mes- 
sage, are in degrees. It is often beneficial to express position in a local coor- 
dinate frame to reduce the complexity of the Kalman filter matrices. The 
following is a brief overview of the equations necessary to convert geodetic 
latitude, longitude to a local coordinate frame, where position is expressed as 
a distance to a defined reference point. 

The first step requires converting the geodetic coordinates expressed in 
latitude, @, longitude, A, and altitude above the WGS8é4 ellipsoid, 4, to the X, Y, 
and Z coordinates of the Earth Centered Earth Fixed (ECEF) Cartesian frame. 
The conversion parameters are given in Table 4.1. 


X =(Ry +h)cos@cosa 
Y =(Ry +h)cos@sina (4.33) 
Z =(Ry(1- e*) +4)sing 


A reference position is defined (denoted by the subscript 0), and the dif- 
ference between the vehicle position and reference position is computed after 
their respective conversion to ECEF coordinates. 


Dx =X - X, 
DY =Y - Y, (4.34) 
DZ =Z- Z, 


A series of rotations based of the reference latitude and longitude pro- 
duces a position in a local coordinate system meters east, north, and up from 
the chosen reference point. 
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a =90- @ 
B =90 +A, 
E =X cos Bcosa +Y cos Bsina- Z sin B (4.35) 


N =- X sina +Y cosa 


U =X sin Bcosa +Y sin Bcosa@+Z cos B 


4.6.2 Accelerometer, Yaw Rate Gyroscope, GPS, and Wheel Speed 


This navigation system will incorporate aspects of the previously discussed 
models to produce a navigation solution. Specifically, longitudinal accelera- 
tion and yaw rate from an IMU will be blended with an odometer and GPS 
measurements contained in the NMEA RMC message. The configuration 
assumes a vehicle traveling on flat ground, zero slide slip, and zero wheel slip. 
The previously defined dynamic models will be used and are defined here for 


completeness. 
0D =a, - b- Q, (4.36) 
b, =p, (4.37) 
W=r- b,- @, (4.38) 
b, =O, (4.39) 


The position is a function of speed and course, and it can be defined 
with the following nonlinear kinematic relationships under the zero side slip 
assumption: 


N =v cosy’ (4.40) 


E =vsiny (4.41) 


A state vector can be constructed containing the necessary states to con- 
trol a vehicle and IMU calibration states. 


x=v b w 6b NET (4.42) 
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The process noise vector is defined as follows: 


o, @, 0. of (4.43) 


a 


O70, Op, 

Equations (4.36) through (4.41) comprise the dynamic model for this 
navigation system. Due to the nonlinearities present, an extended Kalman 
filter must be used. No discretization is required for an extended Kalman fil- 
ter with a nonlinear dynamic model. The model is propagated via numerical 
integration. However, the equations must still be linearized about the latest 
estimate each iteration to produce the required information to propagate the 
estimate error covariance matrix. 


é 0 -1 0 0 Ot 
e L 
é 0 0 0 0 uM 
se * 0 0 ot ies 
BO: e. -G:  BR Geo | 
Sosy O -vsiny 0 ot 
esiny 0 vcosy 0 O O€ 
él 0 0 0 0 Ot 
e L 
er 1 0 O O ae 
€o 0 -1 0 O OL 
L=é C (4.45) 
0 0 0 1 0O OY 
Eo 0 0 0 O Ob 
e L 
60 0 0 0 0 Of 


The continuous process noise covariance matrix is defined as follows: 


&? 0 0 0 0 ot 
€ 5 y 
® of 0 0 0 oO 
€ ° 8 y 
& 0 ZO 2 Ot 10 
Q=% ? { (4.46) 
a 0 0 of 0 0 
D 0 0 0 oO 
@ 0 0 Of 
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Measurements used will be GPS speed, wheel speed, course, north, and 
east. 


z=v mw ww N Ey (4.47) 


The GPS measurements will have direct relationships to the estimated 
states. The wheel speed measurement, obtained by differentiating an odome- 
ter with respect to time, will also have a direct relationship to speed under the 
zero wheel slip assumption. The measurement relationships can be linearly 
modeled in state space format. 


d 0 0 0 0 ou 

Uu 

al 0 0 0 0 0% 
z=) 0 1 0 0 Ote (4.48) 

Uu 

a 00 0 1 04 

®@ 0 00 0 14 


All states are observable when GPS is available. The elements corre- 
sponding to the GPS measurements are set to zero when GPS is unavailable. 
Only the speed and accelerometer bias states remain observable due to the 
wheel speed measurement during those time periods. 

Measurement noise is assumed to be uncorrelated between the mea- 
surements. The covariance matrix contains all the individual measurement 
variances along the diagonal. Note that it must be updated each iteration to 
recomputed the variance of the GPS course measurement. 


Sh 
o 


(4.49) 
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The matrices formulated in this section can be used in the extended 
Kalman filter. The same logical steps and tricks discussed in previous sections 
can be implemented in this filter. 
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Real data was processed in this navigation algorithm. Longitudinal ac- 
celeration and yaw rate were output from an IMU at 32 Hz, a NMEA RMC 
message containing GPS speed, course, and position was logged at 5 Hz, and 
a wheel speed measurement was available at 32 Hz. The vehicle was driven 
counterclockwise around a 2.75-km oval test track with 8° banked turns at 
speeds ranging from 0 to 27 m/s. 

The speed and course data for the entire test are shown in Figures 4.17 
and 4.18. Magnified plots are shown in Figure 4.19, where the Kalman filter 
output can be observed accurately tracking the vehicle’s motion. The noise 
on the heading estimate is slightly reduced from the addition of the position 
measurements to the filter, resulting in a smoother estimate. It can also be 
seen that the 8° bank minimally affects the heading estimate. 

The position estimate sufficiently tracks the vehicle around the test 
track. Figure 4.20 shows the position estimate around the entire test track, 
and Figure 4.21 shows the northeast corner of the test track. The top line is 
the position of the vehicle as it enters the test track, and the bottom line is 
the position of the vehicle as it exits turn four. It is easily seen that the esti- 
mate provides clean information about the vehicle’s position between GPS 
measurements. 

Analysis of the position covariance, shown in Figure 4.22, reveals in- 
formation about the estimate accuracy as the vehicle travels in different di- 
rections. The straight sections of the track run east and west, and the east 
position error is considerably smaller than the north position error while the 
vehicle is on the straight sections. However, the north position error shrinks 
and the east position error grows as the vehicle makes a turn from west to 
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Figure 4.17. The speed profile of the experiment. 
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Figure 4.18 The vehicle heading driving counterclockwise around an oval track. 


south and from east to north. The accuracy variation is due to the persis- 
tence of excitation in the different directions. As the vehicle travels along 
the straight section, the magnitude of the change in position in the east 
is much greater than the noise in the east direction. However, the change 
in the north position is effectively zero, so the noise in the north direc- 
tion dominates that measurement. The correlation between the north and 
east position measurements captures the change in direction as well. The 
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Figure 4.19 Magnification of the speed and heading data shows the estimates accurately 
tracking the vehicle's velocity. 
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Figure 4.20 The position estimate outlines the 2.75-km test track. 


estimates’ correlation varies with course; maximum correlation is reached 
when the course is 45°, 135°, 225°, or 315°, and minimum correlation oc- 
curs when the course is 0°, 90°, 180°, or 270°. 

GPS was artificially removed from 40 to 100 seconds to simulate a 
1-minute outage. Figure 4.23 plots the GPS position estimate during the 
outage. The estimate is plotted with a thick line, and the GPS position is 
plotted with a thin line. The GPS measurements used in the Kalman filter 
are shown by the dots. Initially the estimate tracks the true position well. 
However, the rate gyroscope bias drifts enough during the outage to generate 
a significant position error of 60 meters at the end of the outage. The wheel 
speed measurement improved the performance during the GPS outage as the 
speed estimate remained observable. 
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Figure 4.21 Zooming in on the northeast corner of the test track shows the position esti- 
mate bridging the gap between the low rate GPS measurements. 
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Figure 4.22 


Figure 4.23 
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Variation in the position accuracies occurs due to the changing signal-to- 
noise ratio as the vehicle changes direction. 


0 
—100 
E | | GPS Path 
= —200 e = =Available GPS |- 
s : : == Estimate 
—300 
—400 
—1000 —800 —600 —400 
East (m) 


The position accuracy degrades with the length of the GPS outage. A 60- 
meter error exists at the end of the minute outage. The thin line indicates the 
GPS position, while the dots represent the GPS position used to generate the 
estimate. 
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4.7 Navigation in the Presence of Sideslip 


Vehicle dynamicists often use models to provide detailed vehicle state in- 
formation to the driver or a control system. These models are often intri- 
cate, requiring parameters that are hard or expensive to measure, and can 
be excessively complicated for navigational purposes. Navigation specialists 
sometimes use a simplified vehicle model based on kinematic relationships. 
However, these models sometimes neglect key states such as lateral velocity 
present in the actual dynamics of the ground vehicle operating at normal 
speeds. 

It is important for the designer to be familiar with the operating plat- 
form’s dynamics in its operating environment and to know the limitations 
of the navigation system implemented on that platform. The following sec- 
tions discuss the generation of vehicle sideslip and then present a navigation 
algorithm that utilizes a dual-antenna GPS receiver to incorporate sideslip 
into the model. 


4.7.1 Generation of Sideslip 


An explanation of how sideslip occurs is necessary to understand the errors that 
can potentially arise in a navigation solution. The tire is the vehicle’s interface 
with the road which transfers all drive forces to the ground and generating the 
required lateral forces to turn the vehicle. Using the explanation presented in 
[8], as the tire rolls, parts of the tread either adhere to the road or slide. When a 
force from the vehicle is applied to the tire/road interface, the static coefficient 
of friction is exceeded in local areas on the tread. Areas where the friction coef- 
ficient is surpassed. begin to slide, which generates a force to push the vehicle. 
The generated force is approximated as linear, proportional to slip (a) until the 
tire saturates. When saturation occurs, the force remains constant as no more 
lateral or longitudinal force can be applied to direct the vehicle. The linear in- 
crease and saturation can be seen in the tire curve shown in Figure 4.24. 

The tire cornering stiffness (Cy) is a property that indicates a tire’s per- 
formance when operating within its linear region (i.e., the tire is not approach- 
ing saturation). The cornering stiffness and peak tire force are functions of 
numerous factors such as terrain, normal force, toe, camber, road conditions, 
tire materials, and tread pattern, and values for these parameters are rarely 
known exactly due to the highly complex relationship between these factors. 
Approximations with look-up tables or empirical models, such as Pacjeka’s 
Magic Model or the Dugoff Tire Model, are often used to sufficiently char- 
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Figure 4.24 Two tire curves with the same cornering stiffness but differing friction coef- 
ficients show the available lateral force when a vehicle is on pavement and 
ice. 


acterize the tire [9, 10]. Ultimately, the available vehicle control forces in the 
linear region are a function of the tire stiffness and lateral slip at the tire. 


_, @,7 0 
Qa =tan } eae 
&y,., @ 


The lateral slip at each tire can be translated to the vehicle’s center of 
gravity (CG) to obtain overall vehicle slip, more commonly known as vehicle 
sideslip (B). Sideslip is an angle defined as the difference between the vehicle’s 
course (V) and its heading (yw). This difference can be seen in Figure 4.25. 

Vehicle heading is the direction the vehicle is pointed, while course is 
the direction of the vector created by the vehicle’s direction of travel. Both 
of these angles are referenced clockwise from North. In practice, this angle is 
measured or estimated at the CG, then can be translated to other locations on 
the vehicle for analysis or to extract further information. 

Kinematic relationships between sensors on a vehicle are often used 
to define state equations in a Kalman filter because they require less model 
knowledge and are not affected by to changes in the model, such as vehicle 
mass or tire parameters. The kinematic models presented in the previous sec- 
tions depict the car as if it were on rails by assuming zero lateral velocity, or 
by assuming a proportional relationship between steer angle and yaw rate. 
These assumptions are valid at low speeds for passenger vehicles as the car 
is generating minute slip angles and the lateral velocity is virtually zero. The 


(4.50) 
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Figure 4.25 The difference between heading and course, or sideslip, can be seen in the 
bicycle diagram. 


assumptions break down at higher speeds as lateral velocities are generated 
that invalidate the kinematic model, as will be shown in this chapter. When 
this occurs, a degradation of the performance of navigation or control systems 
may be observed. For these assumptions, speed also must be considered rela- 
tive to the vehicle parameters, or the terrain on which the vehicle is operating. 
A small all-terrain vehicle (ATV) with soft tires maneuvering at 20 m/s on 
pavement could generate the same slip angles as a sports coupe with perfor- 
mance tires traveling 35 m/s. Similarly, a car turning on dry pavement at 10 
m/s would generate very small slip angles, while the same car traveling at the 
same speed would generate significantly larger slip angles if it were on a low 
friction surface such as ice. 


4.7.2 Sideslip Compensation with a Dual Antenna GPS Receiver 


The estimators described in previous sections utilize a single antenna GPS 
receiver and an IMU to track vehicle position, orientation, and speed. The 
NMEA RMC message provides a measurement of a vehicle’s course, speed, 
and position, while the IMU measures a vehicle’s yaw rate and longitudinal 
acceleration. The Kalman filter compares the integrated yaw rate gyro with 
the course measurement, and the integrated accelerometer to the speed mea- 
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surement, to estimate the vehicle’s speed, orientation, longitudinal acceler- 
ometer bias, and yaw rate gyroscope bias. The yaw rate gyroscope provides a 
measurement of the time derivative of heading, 


wer (4.51) 


and the course measurement is the combination of heading and sideslip. 


v=y +B (4.52) 


A heading measurement is not provided in the NMEA RMC mes- 
sage, so the course measurement is substituted for vehicle orientation. Under 
straight or low-speed driving, the integrated yaw rate, or heading, and course 
measurements should be similar. 


v=Bt+Q dt (4.53) 


Small sideslip angles fulfill the small angle approximation, so vehicle 
speed can be approximated with an integrated longitudinal accelerometer. 
However, vehicle speed gains a lateral component when sideslip is nonzero. 


Dx > nde (4.54) 


d =|0|\cos(B); +|2|sin(B) » (4.55) 


Maneuvers producing sideslip, such as an emergency lane change, in- 
troduce a discrepancy in the yaw rate gyro bias estimate because the heading 
from an integrated yaw rate gyro is compared to a GPS course in the innova- 
tion in the extended Kalman filter. The difference between the heading and 
course is perceived as an input and/or measurement error under the assump- 
tions of the previously given navigation algorithms. The error is propagated 
through to the heading and yaw gyroscope bias states. Further complications 
arise because the longitudinal accelerometer in the IMU is measuring a cen- 
tripetal acceleration neglected in the models, and GPS speed from the NMEA 
RMC message is measurement of the norm of longitudinal and lateral veloc- 
ity. These neglected terms can lead to significant state estimate errors if a ve- 
hicle is generating even small sideslip angles. The kinematic relationships that 
account for sideslip should be included in the state equations in the Kalman 
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filter in order to produce a more accurate navigation solution if the appropri- 
ate sensors are available. 

A dual antenna GPS receiver can provide vehicle attitude information. 
A heading measurement can be output from the system, and its accuracy is 
independent of vehicle motion. Another benefit of a dual antenna system is 
the availability of a roll or pitch measurement. A full attitude system with 
three (or sometimes four) antennas can provide all three orientation angles: 
roll, pitch, and yaw. 

The previously defined dynamic model in (4.36) through (4.41) is re- 
defined to handle sideslip. The following model assumes the availability of a 
dual antenna receiver that outputs roll and yaw measurements. The roll rate, 
p, is added as an input and used to estimate the vehicle roll angle. The veloc- 
ity estimate is now broken up into longitudinal and lateral components, and 
centripetal acceleration terms are included. 


Dy dy ~ bye - One FVy(7- by) (4.56) 

base Dy, (4.57) 

Dy Say ~ bey ~ Ony~ Ve(r~ be)- gsin(@) (4.58) 
bry =Ob, (4.59) 

W=r- b.- @, (4.60) 

b, =O, (4.61) 

b=p- by- (4.62) 

by =Wbp (4.63) 


The position remains a function of speed and course, but course is ex- 
pressed as the sum of heading and sideslip. 


; , we 60 
N =vcos(v) =( +0; a cos CW I pied tr (4.64) 
e Sr. 2) 
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ie ee a 
E =vsinv =(02 +02)” sinew taan ge? (4.65) 
e 


Yx, BQ 


A state vector can be constructed containing the necessary states to con- 
trol a vehicle and IMU calibration states. 


x=6, rv, 6 VW be by bp & N ER (4.66) 
The process noise vector is defined as follows: 
‘ T 
@ = Bax Way Wp QO, OWb,». Db, Wo, Ob, al (4.67) 


The dynamics matrix is generated by calculating the Jacobian of the 
vector field consisting of (4.56) through (4.65) with respect to the state vec- 
tor. The result isa 10 © 10 matrix. The nonzero elements are as follows: 


Foy =— 7 +6, (4.68) 
Vx, COS ey +a ang” 5, sin ey +atan es pe 
F Ux, OG Ux. Be (4 69) 
a1 = : 
(2 +; a 
- Vy sin oy +a tan Bo” con cos Oy +a tan ay ast 
F. & by, Og & by, (4 70) 
10,1 = i . 
2,.2\4 
(2 +07] 
Pig =P 34 
Fo. =Fio, (4.71) 
Fi0,2 =Fo,1 


F23 =- gcos() (4.72) 
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(4.73) 
: = = 
Fi0,4 =(02 +0; é cos CY +atan ald mt 
e Sy QO 
Fig =U, 
(4.74) 
Fr 8 =Vy 
Pil 06 F a7 hag = (4.75) 
The process gain matrix is fairly straightforward. 
0 
= 0 
3 0 
; 0 
(4.76) 
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The process covariance is the variances of the noise sources along the 
diagonal of an 8 “ 8 matrix. 


Q =diag(a' o) (4.77) 


Measurements for this system are longitudinal and lateral velocity, head- 
ing (not course), roll, and position. Currently, many of these measurements 
are not available on passenger vehicles but can readily accessible on a UGV 
platform with the right sensor suite. However, obtaining these measurements 
ona highway vehicle is not a distant concept as GPS technology continues to 
become more prevalent in automotive systems. Sideslip has been effectively 
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measured using a yaw rate gyro and GPS, which is currently on many vehicles 
[11, 12]. As previously stated, this algorithm was derived with a dual antenna 
GPS receiver in mind, but the source of a heading or sideslip measurement 
is irrelevant. Once sideslip is known, the GPS velocity measurement can be 
separated into longitudinal and lateral components using (4.55) and used as 
separate measurements. 

The measurement vector is constructed as follows: 


z=. vy @ w NEY (4.78) 
The variances of the measurements are assumed to be uncorrelated, 


even though that is not the case with both lateral and longitudinal velocity 
and position. This assumption provides reasonable results. 


R =diag(o? a7 0, Oy On Of ) (4.79) 


vy 


The measurement matrix is as follows: 
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The navigation model using the dual antenna system was compared to 
the system using a single antenna GPS receiver. An emergency lane change 
maneuver was simulated; the vehicle swerved suddenly, departing from and 
returning to the original lane of travel, as if to avoid an object. Figure 4.26 
compares the heading estimate. The navigation system with the dual antenna 
GPS receiver is able to track the vehicle’s heading, but the system with the 
single antenna receiver cannot. The single antenna receiver provides the 
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Figure 4.26 The heading estimate of two navigation systems is compared during a maneu- 
ver that generates sideslip. The system using the two-antenna system accu- 
rately tracks vehicle heading, while the system using a single-antenna system 
has an error in the heading estimate. 


navigation system with a measurement of vehicle course, or the direction of 
travel, which differs from vehicle heading when the sideslip angle is nonzero. 

Error is also present in the yaw gyroscope bias estimate from the algo- 
rithm neglecting sideslip effects. However, the bias estimate remains close to 
the truth when accounting for sideslip, as seen in Figure 4.27. 

The error is often negligible in many situations, even during maneuvers 
that generate sideslip, for two reasons. The discrepancy will be small, on the 
order of a few degrees, unless complete control of the vehicle is lost. Also, the 
error will not persist after the slippage as incoming measurements will be used 
to correctly calibrate the sensor. 

The heading and yaw gyroscope bias estimate error can negatively im- 
pact the navigation solution in the event of a GPS outage while experienc- 
ing sideslip. The yaw gyroscope will be integrated, with the erroneous bias 
removed, when dead reckoning through an outage. This causes a drastic dif- 
ference in dead reckoning error, as seen in Figure 4.28. 

Another benefit of the algorithm described in this section is the avail- 
ability of a high rate sideslip estimate. Sideslip is the angle between the longi- 
tudinal and lateral velocity components. 


B =tan 1 27S (4.81) 
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Figure 4.27 
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Bias estimates from two navigation systems (one neglecting sideslip effects 
and one incorporating them) are shown during a maneuver that generates a 
nonzero sideslip angle. 


This high rate estimate of sideslip could be used in a stability con- 
trol system on a UGV. Figure 4.29 displays a 50-Hz sideslip estimate 
and a 5-Hz sideslip measurement generated using a dual-antenna GPS 


receiver. 


Figure 4.28 
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Position error during simulated GPS outage is shown. Outages were simu- 
lated just before and during a maneuver that generated sideslip. The algo- 
rithm accounting for lateral motion exhibited a lower dead reckoned position 
error than the algorithm that neglected sideslip effects. 
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Figure 4.29 A sideslip estimate is compared to the measured value. 


4.8 Closely Coupled Integration 


A closely coupled integration routine combines the inertial measurements 
with range measurements from a GPS receiver. The inertial sensors can be 
calibrated using the measured distances to the GPS satellites, and a naviga- 
tion solution can be produced. The algorithm is similar to a range processing 
algorithm used by a GPS receiver to determine user position and velocity. 
The major change is that receiver dynamics are no longer approximated as a 
Markov process with a time constant dependent upon the anticipated receiver 
motion; an IMU provides an actual measurement of the receiver motion. 

One advantage to a closely coupled integration is the availability of 
measurements when a full position solution is not available. Typically, four 
satellites are needed to solve for three position states and a receiver clock bias 
state. When fewer than four satellites are visible, these states are not fully 
observable. A loosely coupled algorithm has no measurement aiding in this 
scenario, and the dead reckoning error can quickly grow. However, the error 
of a closely coupled solution drifts at a reduced rate because the navigation 
processor is still able to incorporate the available range measurements. Note 
that some receivers operate with a reduced state filter when using fewer than 
four observations by constraining the height or clock bias states at the cost 
of an increased error variance. A closely coupled implementation allows the 
designer to incorporate similar logical “tricks.” 

Another advantage to the closely coupled algorithm is realized by the 
individual weighting and processing of the range measurements combined 
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with the inertial measurements containing information about a receiver's 
motion. The navigation solution is less prone to large jumps when satel- 
lites come in or out of view, or when obstructions shadow or reflect the 
signal. 

The algorithm in this section will utilize all outputs from an IMU 
(three accelerations and three rotational rates) and combine them with GPS 
pseudorange and Doppler frequency measurements. The pseudorange is a re- 
ceiver to satellite range measurement, and the Doppler frequency can be used 
to form a pseudorange rate, or a receiver to satellite range rate measurement. 
“Pseudo” is used to indicate the measurement is not a true range or range 
rate due to the presence of errors. This algorithm is also derived assuming a 
discrete time system. This gives the reader a look at an alternative method to 
develop navigation system models. It will utilize an extended Kalman filter 
with a nonlinear time propagation and a nonlinear measurement update. 

A strap-down IMU must be mechanized before integration with the 
GPS range measurements. Its acceleration and turning rate measurements 
will be about the IMU frame. The navigation frame and IMU frame are usu- 
ally noncoincident, and therefore the inertial measurements must be trans- 
formed from the IMU frame to the navigation frame. An Euler angle-based 
approach is presented, although other methods are commonly used. The 
Euler angle approach is fairly intuitive, but a singularity exists when the pitch 
angle is 90°. If this presents a problem to the designer, an alternative method, 
such as a quaternion-based approach, should be sought. 

The algorithm presented in this section uses the ECEF frame as the 
navigation frame. Therefore, the Euler angles (@, @, W) correspond to the 
three rotations necessary to rotate inertial measurements from the IMU 
frame to the ECEF frame. The inertial frame is assumed to be coincident 
with the body frame, so the body to ECEF frame angles can be expressed 
as Qj, Of and yf. Note that the Greek letter psi is not to be confused with 
vehicle heading; the Euler angle and vehicle heading do not have to have a 
direct relationship. 

A coordinate transformation matrix is constructed in order to express 
the IMU accelerometer measurements in the ECEF frame. It consists of oper- 
ations on the sine and cosine of the angles. The notations s(-) and c(-) denote 
the sine and cosine of an angle, respectively. 


GOcy -chsytspsOcy — shsy tcp 50 cy 
Gy =50 sw chcytshsOsyw —- shswtco sO sy 
& - 50 spcO ch cO 


(4.82) 


(a eoON aa aut aut 


132 GNSS for Vehicle Control 


The expression of the angular rates of the body frame in the ECEF 
frame requires the time derivative of the rotation matrix. 


i ps0 chsOL 
é cO cO : 
M, =@ ch - sh U (4.83) 
€ 1) co | 
= cO cO t 


Notice the division of terms by the cosine of the pitch angle. Together, 
these matrices are used to express the IMU output in the ECEF frame. 
Knowledge of the vehicle’s original position and orientation is necessary to 
initialize these matrices. 

A notational difference exists between this section and previous sec- 
tions. This difference is due to the expression of the same measurement in 
multiple coordinate systems. Using the notational style in [2], a generic vari- 
able can be expressed as 


XOB (4.84) 


where the vector x is the kinematic relationship from the o-frame to the 
B-frame expressed in the y-frame. 

The accelerometer output is denoted by the vector f, and it contains 
the three measurements about the local X, Y, and Z axes. This variable is 
chosen to reflect the fact that the accelerometer is technically outputting 
a measurement of specific force. Forces that move the accelerometer bend 
or move a proof mass, and the output is a ratio of the measured force to 
the proof mass. A superscript denotes the frame in which the measurement 
is expressed. For instance, f* denotes the specific force measurement ex- 
pressed in the ECEF frame, and f° denotes the specific force measurement 
in the body frame. 

Rotational rate vectors are expressed using the vector @. The IMU mea- 
sures both the body and Earth rotation, but these two can be separated. The 


body rotation vector about the ECEF frame expressed in the body frame is 
defined as follows: 


O.=p q rl (4.85) 
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where p, g, and rare the body roll, pitch, and yaw rates. The Earth rotation 
vector about the inertial frame expressed in the ECEF frame is defined as 
follows: 


&=0 0 a] (4.86) 


where @, is 0.00007292115 rad/s. A skew symmetric form of the Earth rota- 
tion can be also be written. 


0 -@, 0 
Wi. =, 0 0 (4.87) 
0 0 0 


The Euler angles are contained in a vector denoted by a, which is known 
as the attitude vector. 


Zz I 
a =, 6; vay (4.88) 


The specific force expressed in the ECEF frame is integrated to pro- 
duce the ECEF velocity and ECEF position. The measurement is corrected 
for gravitational, centripetal, and Coriolis effects so only the specific force 
corresponding to the translational motion of the receiver is integrated. The 
accelerometer bias is removed from the specific force in the body frame, and 
the resulting quantity is rotated to the ECEF frame. The variable w is used 
to indicate the presence of white noise. The gravity, centripetal, and Coriolis 
terms are then removed. 


= 


faa (7! - by, - iop,) +a(7*. ) - 2Wh0%),, (4.89) 


The middle term represents the gravity model. This accounts for the 
gravitational and centripetal effects. Various gravity models based on the us- 
er’s location and altitude can be implemented. A simple model using the stan- 
dard Earth gravitational parameter, GM, where GM = 398,600,441,800,000 


m?/s’, is shown here. 


G5) = Soa - Wer (4.90) 
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Once the specific force at time & is determined, the position, velocity, 
and attitude states can be discretely propagated using the sample rate, ¢. 


2 


SO a Se =e Fe t 
Toby lob ra 1 t+ fy 2 (4.91) 
me le Fe 
Y ebp =. Ubi 1 +f; t (4.92) 
dt = ap i+.Mj(o%, bg, igs) t (4.93) 


As with the accelerometer measurement, the rate gyroscope bias is re- 
moved before transforming the measurements into the navigation frame. The 
six bias states, three accelerometer biases and three rate gyroscope biases, are 
modeled as white noise. 


be, = by, i + Wop, (4.94) 


ben = Dep + Phy, (4.95) 


Two additional states are necessary when processing pseudorange and 
pseudorange rate measurements. The oscillator on the receiver driving its 
clock will have a phase and frequency error, which produces a bias and drift 
in the clock output. A common two-state model derived in [6] is used to 
capture the clock errors. It assumes both the bias and drift exhibit a random 
walk over time, and that the drift drives the bias state. The speed of light 
constant (¢c = 299,792,458 m/s) is multiplied through to convert the time to 
units of distance. 


cOty, = Coty, , +cOlye t+woy, (4.96) 
cOty, = cOty, , + Wéi, (4.97) 
There are 17 total states representing a three-dimensional position, ve- 


locity, and orientation of the receiver, plus the six IMU calibration states and 
two receiver clock states. 


= ia 
x= 7, Dy, a br by ct cot (4.98) 
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Although the nonlinear system model is propagated with (4.91) through 
(4.93), the state transition matrix is still required to propagate the error cova- 
riance. The state transition matrix is determined by calculating the Jacobian 
of the vector field of nonlinear functions with respect to the state vector. The 
noise terms are ignored as they are nondeterministic; they will be accounted 
for in the process noise gain and covariance matrix. The nonzero blocks of 
the state transition matrix are shown here. 


e t 
Dr? 6 GM _._.-r GM 4 
F 1:3,1:3 =[+ 2 3 ell? renee 7 ell? fie (4.99) 
@ "eb "eb t 
é GM GM L 
e weveT L 
4:6,1:3 = E° a 5 vb eb ns = 3 - fee ( (4.100) 
8 |’ eb e 
F 1:3,4:6 =D-(7 : W. Dr) (4.101) 
F 4.64.6 = - 2W,,Dr (4.102) 
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g (cc) fy +(- s6c6) f. E 
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E (sosy +cosOcw) fy +H(cosy - sbsOcy) f, : 
F 4.6,7 =D g- sos +cosOsw) fy ii cocy - sosOsw) ft U (4.106) 
g (chc0) fy + (- sbcO) f, b 


q- socw) fic +H(spcOcw) fy +(cocOcw) fr U 
F 469 =Deal-sdsy) fe HsbcOsy) fy HedcOsy) fi. (4.107) 
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F 7.9,9 =; (4.111) 
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F 131012 = - Cy (4.112) 
F io12,1012 =f (4.113) 

F 7.91315 =- DM; (4.114) 

F 13151315 = (4.115) 

F 16:17,16:17 “5 (4.116) 


The process noise on the states comes from noise in the IMU and 
the receiver clock. The IMU noise is modeled as independent and uncorre- 
lated white noise. The clock noise model was derived in [6, 13] and requires 


knowledge of the clock quality to determine the spectral variance of the bias 
and drift. 


sp =27h- re? (4.117) 
sd = 22 (4.118) 


Values for Ay and 4., can be obtained from an Allan variance plot of 
the clock output. Values listed in [13] for several clocks of varying quality are 
listed in Table 4.2. The process noise vector can be defined with statistics of 
the IMU noise and bias drift and receiver clock quality. 


Table 4.2 
Power Spectral Density Coefficients for Various Clocks 
Clock Type hy h, 
Compensated crystal 2° 10°"9 2° 1072 
Ovenized crystal 8° 10° 4° 10° 
Rubidium 2° 10°” 4° 10° 
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_ 7: 2 a 2 2 
Qi:12,112 =diag(o2, Cin Cag Tuy ) (4.119) 
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The process gain matrix is determined by calculating the Jacobian of 
the vector field generated by (4.82) through (4.90) with respect to the process 
noise vector. The elements of the process gain matrix corresponding to the 
clock states are set to identity. 


Dy? 
G313 = - CG; (4.121) 
Gi.613 =- Dec; (4.122) 
G,9,4.6 =- DiM; (4.123) 
Gio79 =- Dt? Mj (4.124) 
G315,7:9 =Del (4.125) 
De 
G:3,10:12 = 2 C; (4.126) 
Ge.6,1012 =- Dr*C; (4.127) 
Goo12,1012 =Del (4.128) 
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Measurements in the closely coupled system consist of the range in- 
formation computed in the GPS receiver. Specifically, the pseudorange and 
pseudorange rate are used. The pseudorange measurements are usually out- 
put directly by the receiver. However, the pseudorange rate is typically deter- 
mined by manipulating the Doppler frequency measurement. 


=- Afp (4.130) 


In (4.136), the pseudorange rate is a function of the carrier wavelength, 
A, and the Doppler frequency, fp. The phase information can be used in lieu 
of the pseudorange rate, but using both does not add information to the sys- 
tem because the phase measurement is the integral of the Doppler shift. 

The measurement vector is denoted by z, and it contains all of the pseu- 
dorange and pseudorange rates to be processed. The system designer must 
account for the change in size of this vector as the number of observations 
fluctuates. 


z=[p pl (4.131) 


The nonlinear relationship between the states and measurements, or 
measurement model, is expressed as follows: 


é |. 
e\5- ie o> its Utc6r 
A(x) “8g if a, - a +66 
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The subscript 6 gives reference to the body or receiver position and ve- 
locity, and the subscript s is used to denote the GPS satellite position and ve- 
locity. Computing the Jacobian of A(x) with respect to the state vector yields 
a linearized measurement model that utilizes the user to satellite unit vectors. 
The elements relating the velocity measurements to the position states are 
small enough that they can be assumed to be zero. 


(4.132) 
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Measurement accuracy is determined by the bandwidth of receiver’s 
tracking loops, signal quality, and anticipated multipath effects. Expressions 
of the accuracy of the delay and frequency lock loops (DLL and FLL, respec- 
tively) as a function of the carrier-to-noise ratio, C/No, expressed in hertz, are 
given in [4, 5]. 


(4.134) 
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The measurement errors consist of the tracking loop errors plus ad- 
ditional terms. The term « is used to inflate the error bounds to account 
for unmodeled terms, such as multipath and any residual atmospheric ef- 
fects. The term ff is the dynamic stress error and is a function of track- 
ing loop bandwidth and loop order. The accuracy of the pseudorange 
and pseudorange rate measurements can be modeled with the following 
expressions. 


Table 4.3 
Tracking Loop Parameters 


Parameter Value Unit 
B,, code loop noise bandwidth 2 Hz 

d, correlator spacing 0.5 Chips 
f,, dynamic stress error 3 m/s 
K,, unmodeled range error 5 m 

A, carrier wavelength 0.1902 m 

A,, code chip width 293.05 m 


T,, predetection integration time 0.005 


n 
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Figure 4.30 Pseudorange and pseudorange rate measurement noise increases as the 
C/N, decreases. 
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Figure 4.31 GPS and GPS/INS position plotted in the NED frame shows the improvements 
of adding inertial data into the filter; jumps in the position are filtered out. 
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Figure 4.32 Vehicle velocity is provided by the closely coupled algorithm in the ECEF 
frame at a high rate. 


Reasonable values and definitions of the parameters in (4.122) through 
(4.125) obtained from [5] are given in Table 4.3, and Figure 4.30 displays 
expected accuracies as a function of C/N in dB-Hz using those parameters. 

Data was collected on a vehicle traveling through a typical suburban en- 
vironment using an automotive grade IMU and a GPS receiver. GPS data was 
logged at 5 Hz, and IMU data was logged at 50 Hz. Figure 4.31 shows the 
vehicle position plotted in an NED frame of reference. The 50-Hz position 
estimate from the closely coupled system is superior to the 5-Hz GPS only 
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Figure 4.33 Estimates of the Euler angles from the body frame to ECEF frame are provided 
at a high rate by the closely coupled GPS/INS navigation system. 
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solution as jumps in the position are eliminated as the vehicle travels between 
three-story buildings. A high rate, three-dimensional velocity estimate is also 
provided by the closely coupled system. Figure 4.32 depicts the vehicle veloc- 
ity in the ECEF frame. Figure 4.33 shows the Euler angles from the body 
frame to the ECEF frame as estimated by the closely coupled algorithm. 
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Vehicle Dynamic Estimation Using GPS" 
David M. Bevly, Robert Daily, and William Travis 


5.1 Introduction 


The state feedback that will be described in Chapter 6 requires knowledge 
of all the states in the vehicle model. Some of these states (specifically lateral 
velocity or sideslip) are often not measured and therefore must be estimated. 
Additionally, the full state feedback assumes that vehicle parameters such as 
tire cornering stiffness are known. This chapter demonstrates various meth- 
ods for estimating vehicle sideslip (for use in the full state feedback control 
algorithm presented in Chapter 6) as well as estimating critical model pa- 
rameters. In order to perform vehicle control, both navigation states (such 
as vehicle position and orientation) and feedback control states (such as lat- 
eral velocity, yaw rate, and steer angles) must be obtained, either from direct 
measurements or observers. Because observers for estimating missing states 
in full state feedback control algorithms are well known, this chapter focuses 
primarily on the estimation of the lateral velocity and vehicle parameters that 
effect the lateral velocity. Note that the state estimation algorithms presented 


*Reproduced with the permission of Inderscience Enterprises Limited, who retain the copy- 
right, from the following source: Daily, R., W. Travis, and D. M. Bevly, “Cascaded Estima- 
tors to Improve Lateral Vehicle State and Tire Parameter Estimates,” International Journal of 
Vehicle Autonomous Systems, Volume 5, Number 3-4, 2007, pp. 230-255. 
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in this chapter can be done independently or in conjunction with the naviga- 
tion estimators presented in Chapter 4. However, it has been shown that it 
is often best to separate the estimation of the navigation states and feedback 
control states [1]. 


5.2 Sideslip Calculation 


The body sideslip angle (8 in Figure 5.1) is the angle between the longitudi- 
nal and lateral velocity components at the center of gravity (CG). 


4: 
B =tan SV Ms (5.1) 


A typical method to measure sideslip on test vehicles is using an optical sensor 
to measure the lateral and longitudinal velocity. However, these sensors are 
expensive and performance can be surface dependent. For these reasons, on 
production vehicles, this state is not measured. A newer method to measure 
sideslip is using GPS. The sideslip angle can also be defined as the difference 
between the vehicle heading and direction of travel (course) measured at the 


ce. 
B=v-y (5.2) 


GPS provides a course measurement. However, the vehicle heading must 
still be determined. Heading can be estimated using a yaw gyro. The gyro is 


Figure 5.1 Lateral bicycle model showing vehicle sideslip angle. 
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initialized and its bias estimated during periods of straight driving and then 
integrated during turning maneuvers to provide the vehicle heading [2]. 


5.3 Vehicle Estimation 


The GPS velocity based course measurements can be combined with the yaw 
rate gyroscope measurement through two types of estimators, a kinematic 
estimator or a model-based estimator. The purpose of combining the GPS 
measurements with the gyroscope measurements is to provide high-update 
rate, unbiased, accurate estimates of the lateral vehicle dynamics. This chap- 
ter utilizes a Kalman filter to integrate the GPS and IMU measurements to 
provide the state estimates for the vehicle. Note that one could utilize any 
type of estimator such as a traditional linear estimator with desired closed- 
loop dynamics designed based on the characteristics of the system dynamics. 
The Kalman filter does have the distinct advantage of providing the accuracy 
of the state estimation (assuming all the parameters in the estimator are cor- 
rect) in the state estimation covariance matrix (P). 
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Figure 5.2 Instrumented Infiniti G35 sedan. 
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5.4 Experimental Setup 


The methods described in this chapter are experimentally tested on the fully 
instrumented G35 sedan shown in Figure 5.2. The G35 is equipped with 
a dual antenna Novatel Beeline receiver that provides 5-Hz updates of ve- 
hicle heading, roll, speed, and course. It is also instrumented with a lateral 
accelerometer and yaw gyroscope recorded at 30 Hz. Additionally, wheel 
speed and steer angle are recorded from the onboard sensors through the 
CAN bus at 30 Hz. With the exception of the dual antenna GPS system, each 
of these sensors is available on any vehicle with electronic stability control 
(ESC). The methods presented in this chapter show the benefit of adding 
dual antenna GPS to ESC. The vehicle’s mass (m) is 1,528 kg; the yaw mo- 
ment of inertia (/,) is approximated as 2,400 kg m? [3]. 


5.4.1 Test Scenarios 


Two experimental test scenarios are used to validate the methods presented 
in this chapter: hard cornering on a low-{l gravel surface and limit driving 
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Figure 5.3 Low-y experiment steer angle and yaw rate. 


Vehicle Dynamic Estimation Using GPS 149 


on asphalt. The low- surface experiment steer angle and resulting yaw 
rate are shown in Figure 5.3. This experiment is run at an average speed of 
6 m/s. 

The asphalt scenario consists of three segments. The first segment is a 
high-speed, steady state cornering experiment performed at 37 m/s around 
a 150-m radius turn banked at 8°. As in (3.18), the bank angle is typically 
neglected in vehicle models. The second segment is a lane change performed 
at 20 m/s. The final segment is a slalom at 25 m/s. The steer angle and yaw 
rate for this experiment are shown in Figure 5.4. 


5.5 Kinematic Estimator (Single GPS Antenna) 


One of the current ways to combine the GPS heading measurements with 
the information provided from the yaw gyroscope is through a kinematic or 
indirect Kalman filter [4-6]. The term kinematic comes from the fact that 
the model used by the estimator is based solely on the kinematic relationship 
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Figure 5.4 Asphalt experiment steer angle and yaw rate. 
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between the sensors, and not a dynamic model of the vehicle. The state space 
model for the kinematic Kalman filter is given in (5.3): 


éyu ® -itéy udu 
: u=6 W) ae A tf WwW 
Povo ) Oar G Oy 
(5.3) 
e Uu e Uu 
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In the kinematic Kalman filter, the two states being estimated are the 
heading of the vehicle and the bias on the yaw rate gyroscope which is mod- 
eled as a random walk. The yaw rate acquired from the gyroscope is used as 
an input into the system. During straight driving, the observation matrix 
(C) is set to [1 0] in order to estimate the bias on the gyroscope by using 
the GPS course measurement. GPS course is the heading of the vehicle plus 
sideslip which is assumed to be zero during straight driving. The observation 
matrix is then set to [0 0] during turning or during periods when GPS is 
lost. Setting the observation matrix to zero during turning effectively turns 
off the gyro bias estimation and integrates the yaw gyroscope (with the bias 
removed) in order to provide an estimate of the true vehicle heading. It is 
important to note that this method can lead to estimation errors in the pres- 
ence of gyroscope scale factor errors or during periods of long integration. 
Although the kinematic Kalman filter does not provide a direct estimate of 
vehicle sideslip, an estimate of sideslip can be obtained by comparing the 
difference between the GPS course angle and the estimated heading of the 
vehicle from integrating the bias-free gyro. The equation for this sideslip es- 
timate can be seen in (5.4). 


B =vors - Wee (5.4) 


The measurement noise and discrete process noise covariance matrix for the 
kinematic Kalman filter are given in (5.5): 
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Note that the estimator dynamics change with these parameters [6]. 

Note that although the kinematic Kalman filter is able to account 
for the bias present on the gyroscope, the method is unable to account for 
the presence of a scale factor error on the gyroscope. Although methods to 
estimate the gyroscope’s scale factor error exist [7], any error in the gyro 
scale factor error will lead to a growth in the slip angle estimate error dur- 
ing turning. Also, with a single antenna receiver, the method cannot ac- 
count for roll or road grade errors, which can lead to a false estimation of 
sideslip. 


5.6 Kinematic Kalman Filter (Dual Antenna) 


The single antenna estimation technique presented in the previous section 
has several drawbacks. Estimating an integrated signal is an unobservable 
process; mathematically the estimate error cannot be bound. Practically, this 
means errors such as incorrect biases or scale factors can cause large errors in 
the sideslip estimate. Another problem is aligning the time that each mea- 
surement was taken. Inherent delay occurs between the time a GPS mea- 
surement is taken and the time the measurement is received by a computer. 
Depending on how the gyroscope measurement is taken, it may have a delay 
as well. Because vehicle sideslip is generally small (on the order of 1—2°), the 
two signals being subtracted in (5.2) must be aligned at precisely the same 
time to produce an accurate measurement [5]. Without an external signal 
(such as the GPS pulse per second), it is not easy to programmatically line up 
the two measurements. 

Therefore, an improved method of estimating the vehicle sideslip angle 
can be performed using vehicle heading measured using two GPS antennas 
[8, 9]. By utilizing two antennas, heading estimate errors due to both gyro 
integration (arising from bias estimate errors, scale factor, integration rou- 
tine, and integration of a noisy signal) and synchronization of the GPS/INS 
measurements are eliminated. The GPS velocity measurements (including 
course) have a delay that is not present in the position measurements (in- 
cluding heading). The GPS velocity vector is calculated by comparing con- 
secutive phase measurements of the carrier wave. When consecutive phase 
measurements are compared, the measurement has a half sample delay. This 
delay is similar to that introduced by a backward difference approximation. 
Unlike the GPS/IMU synchronization, this delay is easy to account for 
programmatically. 


152 GNSS for Vehicle Control 


Sideslip Angle (deg) 


0 5 10 15 20 25 30 
Time (s) 


Figure 5.5 Asphalt experiment GPS slip angle validation with optical sensor. 


The two methods (GPS and optical) are compared for the asphalt ex- 
periment in Figure 5.5 and match quite well; for the sake of scale, the slalom 
is not included. This validates the GPS method to compute sideslip presented 
above. Further validation using the optical sensor can be found in [10, 11]. 
For the remainder of the chapter, the GPS approach will be used for the 
sideslip measurement. 

The first step of the cascaded estimation approach utilizes a kinematic 
Kalman filter to combine the GPS and inertial measurements to provide 
high-update rate, unbiased, accurate estimates of the lateral vehicle dynam- 
ics [5]. For this filter, the linearized form of the Kalman filter described in 
the Appendix is used to estimate the states. The models for the gyroscope 
and lateral accelerometer described in Chapter 1 can be combined with the 
measurements from a two-antenna GPS system to form the kinematic state 
space relationships given next. 
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The estimator adjusts the accelerometer bias estimate to compensate for any 
roll measured by the lateral accelerometer. However, the two-antenna GPS 
system provides a roll measurement that can be used to correct the lateral 
acceleration. 
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The sensor noise and process noise covariance matrices for this system 
are given here. 
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T, is the sample rate of the Kalman filter inputs. Figure 5.6 shows the estimates 
of sideslip and heading, for the low- and asphalt tests described previously. 
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Figure 5.6 (a, b) Estimates using the kinematic Kalman filter. 


5.7 Tire Parameter Identification 


Using the state estimates from either kinematic Kalman filter, the tire curve 
for each axle can be generated and tire parameters estimated. Because no small 
angle approximations are used in the kinematic Kalman filter plant model, 
the estimates are valid beyond the linear region of the tire. Several research- 
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ers have produced experimental tire curves using various methods (such as 
steering wheel torque) to estimate the lateral force at the tire [12-15]. Other 
researchers have developed methods to estimate peak coefficient of friction 
[16, 17], including methods that utilize GPS [18]. In this chapter, simple 
Newtonian dynamics are used to accomplish the force estimation. The lateral 
acceleration and yaw rate estimates can be used to determine the front and 
rear axle lateral forces. 


= F, =ma, =F), +Fy cos(6) 
Q s (5.9) 
a M, =1,W =- bF,, +aFy cos(6) 

Several assumptions are made in these equations. First, longitudinal 
forces are ignored. The only longitudinal force that might appear is from the 
front tire due to the steer angle. Unless the vehicle is braking, this force will be 
negligible for rear wheel drive vehicles (such as the G35). It is possible to add 
a longitudinal acceleration/force equation and make an assumption about the 
brake bias to include braking and driving forces. If the vehicle is undergoing 
significant acceleration or braking, the additional forces should be included 
so the lateral estimation does not become corrupted by the neglected longi- 
tudinal terms. Including longitudinal forces also allows the longitudinal tire 
curve to be predicted; however, this is beyond the scope of this chapter. 

The lateral forces in the equations are per axle, not per tire; the lateral 
force on an axle is simply the sum of the lateral forces from each of the tires 
on that axle. The equations include the second derivative of heading. To 
determine this measurement, the yaw rate gyroscope must be numerically 
differentiated; the differentiation may add lag and noise that can corrupt the 
estimation. Using the tire sideslip measurements presented above and these 
lateral forces, it is possible to predict the lateral tire curves for the vehicle. 
The tire curves can then be used to estimate parameters for the various tire 
models available. 

Although many researchers have estimated tire cornering stiffness on- 
line using linear tire models, to predict the tire behavior outside the linear 
region, a more complicated model such as the Dugoff model, discussed in 
Chapter 3, must be used. The Dugoff model has two parameters to iden- 
tify for each tire: cornering stiffness and peak lateral force (peak coefficient 
of friction multiplied by vertical load). As with the linear model, it is safe 
to assume the inner and outer cornering stiffnesses are equal; the inner and 
outer coefficients of friction are also equal. However, this model includes 
(and is not linear in) vertical force; assuming no weight transfer is not 
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always practical. To allow the model to capture a wider range of vehicle be- 
havior, a more complicated tire model may be used, such as Pacjeka or Fiala 
models. 

Because the Dugoff model is not linear in vertical force, it is not im- 
mediately apparent mathematically that it is possible to identify a “lumped” 
peak force for the axle. This problem can be visualized in the transition re- 
gion of the model. With this model it is possible to have the outer tire in the 
linear region (A > 1) and the inner tire beginning to saturate (A < 1) due to 
more vertical load on the outer tire. However, it is normally safe to assume 
that the concept of the Dugoff model works for an axle as well as for a tire; 
the axle will have a cornering stiffness and a peak lateral force. The Dugoff 
model of the axle captures these two characteristics. This assumption may 
break down in vehicles with large roll angles, however, particularly if the roll 
dynamics are not significantly faster than the yaw dynamics (i.e., the weight 
transfer for a given slip angle cannot be considered constant). In this case, the 
peak lateral force for the axle would not be constant. 

For vehicles with large amounts of roll, this presents opportunities as 
well as difficulties. If cornering stiffness and coefficient of friction are as- 
sumed equal for the inner and outer tires and the total vertical load on all the 
tires is constant (vehicle weight), it should be possible to identify the vertical 
load on each tire as well as the tire parameters by summing the lateral forces 
for each axle. However, the vehicle used in this chapter does not exhibit sig- 
nificant weight transfer; thus, there is not enough excitation to identify the 
tire loads separately. 

Because the Dugoff model is nonlinear, simple techniques such as re- 
cursive least squares (RLS) do not work to identify the parameters. However, 
researchers have used a modified Dugoff model to incorporate RLS [19]. A 
more complicated nonlinear technique, such as an extended Kalman filter, 
must be used for the nonlinear Dugoff model, where the states (i.e., param- 
eter estimates) vary with time. Both the Kalman filter and extended Kalman 
filter are presented in the Appendix. 
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The measurement is lateral force, so the output matrix (C) is the gradi- 
ent of the Dugoff model with respect to the parameters. 
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The process noise covariance (Q)) acts like a forgetting factor, allowing 

the estimated parameters to change over time. For this work, the process and 


measurement noise covariances are chosen to be 


R, =(1,000 Ny 


60? 480,000 N/rad)* 0 : (5.12) 
é 0 (15,000 N)’¢ 


Another way to think of the recursion algorithm is as recursive least squares 
where the regression matrix is defined like the output matrix in the Kalman 
filter. 

The parameter identification algorithm is validated on the two experi- 
mental runs described above. Figure 5.7 shows the asphalt experiment. There 
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Figure 5.7 Estimated tire curves from the asphalt experiment. 
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appears to be multiple fits due to the estimation algorithm adapting over 
time. The raw data also seem to have this behavior implying the parameters 
are varying with time. In particular, the rear tire saturates at approximately 
7 kN but distinctly goes above this lateral force at 5° of slip. This discrep- 
ancy could be due to weight transfer. Slip angles beyond 7° are only seen 
near the end of the slalom portion. This maneuver is violent enough that 
weight transfer was significant. The other two portions of the experiment 
(steady state cornering and lane changes) are not that violent and thus do 
not have the weight transfer. The estimation algorithm is adaptive enough to 
capture the different behaviors at different times during the experiment. 

Figure 5.8 shows the parameter estimation throughout the asphalt ex- 
periment. The Kalman filter (recursive) is compared to a nonlinear batch 
least squares performed over the entire experiment. For the most part, the 
recursion converges near the batch estimation. The notable exception to this 
is the rear cornering stiffness; the recursion remains lower than the batch es- 
timation. Although this difference looks significant as plotted, the difference 
in slope represented on the tire curve is not very noticeable. Either could be 
judged as a valid fit. 

The peak lateral force estimate portrays an aspect of the Kalman filter 
that is quite useful in this application. Most parameter identification schemes 
require a switch to determine when to estimate solely the cornering stiffness 
versus estimating both parameters (i.e., when the tire is beginning to satu- 
rate). When the tire is in the linear region (A > 1), the Dugoff model (and 
thus the Kalman filter output matrix) does not contain the peak force; this 
parameter is unobservable. When the Kalman filter encounters an unobserv- 
able state, it sets the gain on that state to zero in this case, meaning hold the 
parameter constant. Therefore, when the tire is operating in the linear region, 
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Figure 5.8 Estimated tire parameters from the asphalt experiment. 
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Figure 5.9 Estimated tire curves from the low-y test. 


the Kalman filter automatically holds the peak force at the last estimated 
value. This is the reason for the many plateaus in the peak force estimate. 
The tire only enters the nonlinear region for short periods at a time; the first 
of these is exiting the corner. During these short intervals, the peak force 
estimate is updated; otherwise, it is simply held constant. 

Figure 5.9 shows the measured tire curve along with the estimated 
Dugoff fit for the low-u experiment and Figure 5.10 shows the resulting 
parameter estimates (note the difference in both the cornering stiffness and 
peak force from the asphalt experiments). Filtered measurements are shown 
along with the unfiltered measurements and curve fit; the curve fit utilizes 
the unfiltered measurements. The reason for the filtered measurements is to 
show graphically that the curve fit appears to identify the most reasonable 
parameters for the tire. As with the asphalt experiment, the parameters appear 
to vary with time (there appear to be multiple curve fits); this is due in part to 
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Figure 5.10 Estimated tire parameters from the low-y test. 
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weight transfer as it was in the asphalt experiment. The changing parameters 
are also due in part to the irregularity in the gravel surface itself. In addition, 
the measured force (even after filtering) is very noisy due to the rough gravel 
surface, but the estimation scheme effectively averages the noise out. The 
parameters identified in this experiment are successfully used in the following 
model-based filter, showing that even in the presence of a large signal-to- 
noise ratio the identification and estimation schemes perform well together. 

Another important consideration when estimating the tire parameters 
is the initial guess of peak force. To obtain an accurate estimate, the initial 
peak force guess must be higher than the actual value. In an extreme sense, 
if the peak force estimate is too low, the estimator is constantly receiving 
measurements of lateral force above the peak and so continually moves the 
estimate of peak force up to the measured value. However, the Kalman 
filter also believes the estimate cannot change quickly (based on Q)), and so 
it becomes “harder” to move the estimate. Ultimately, the estimate of peak 
force will always be lower than the truth and even the highest measured 
forces. 

This problem is easy to circumvent by initially guessing the peak force 
unreasonably high. This does not cause any problems in the estimator be- 
cause while the tire is in the linear region, the peak force does not come into 
play. As soon as the tire begins to saturate for the first time (¢ = 15s in Fig- 
ure 5.8), the peak force estimate drops immediately to a reasonable value. 


5.8 Model-Based Kalman Filter 


As discussed in Chapter 3, the understeer gradient (Kys) defines whether the 
vehicle is understeer (Kyjs> 0), oversteer (Ky < 0), or neutral steer (Kys = 0). 
The vehicle’s understeer characteristics determine the change in steer angle 
required to hold a steady state turn as velocity increases [20]. In a neutral 
steer vehicle the understeer gradient is zero; the state transition matrix from 
(3.18) simplifies to 
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Note this allows yaw rate to be independent of sideslip, but sideslip is still 
dependent upon yaw rate. This results in the sideslip being unobservable with 
only a yaw rate measurement. 


5.8.1 Linear Tire Model 


Once the tire parameters have been identified, the GPS measurements can 
be combined with the inertial sensors using a model based estimator. The 
estimator’s state space model is derived from the linearized bicycle model 
given in (3.18). This Kalman filter form is called a model-based estimator 
because the vehicle model is used in the state matrices for the estimator. The 
state space equations for the model-based estimator, including the heading 
and gyro bias, are shown in (5.14). 


The output equations for this model (as well as which states are ob- 
servable) depend on the sensors used. Table 5.1 provides the various states 
(and the associated C and D matrices) that are observable given the mea- 
surements available to the model based estimator. Farrelly has shown [21] 
how to incorporate a lateral accelerometer measurement into the model 
based estimator that results in the following state space description of the 
system. If used, the lateral accelerometer measurement must be compen- 
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Table 5.1 
Kalman Filter Sensor Options 
Case Sensors CMatrix DMatrix Observable 
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*If the vehicle is not neutral steer. 
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sated for roll as shown in (5.7). Additionally, if performing this state esti- 
mator in conjunction with the navigation estimator in Chapter 4, the bias 
compensated gyro can be used for the yaw rate measurement. This elimi- 
nates the need to reestimate the gyro bias and also allows for the estimation 
of a steer angle bias. 

These estimators provide a direct estimate of the sideslip (8), yaw rate 
(r), and vehicle heading (yw). Some configurations estimate the gyroscope 
and/or accelerometer biases (4,,,. and Occ). The input into the estimators 
is the steer angle at the wheel (6). Measurements include yaw rate plus bias 
(from a gyroscope), the course angle (from GPS), vehicle heading (from 
two antenna GPS), and lateral acceleration plus bias (from a lateral ac- 
celerometer). Note, as shown in (5.13), vehicle sideslip is not observable 
using only a yaw rate measurement if the vehicle model is neutral steer. 
However, sideslip remains observable with GPS measurements for a neutral 
steer vehicle. 

The discrete processes noise covariance is shown in (5.15). 
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Note that the actual transformation from process noise on the steer angle 
to state process noise must propagate through the input matrix. However, 
it may be better to “hand-tune” the process noise covariance matrix to ac- 
count for model parameter errors in the estimator model, resulting in the 
covariance above. Also, note that the estimator dynamics will change with the 
magnitude of the noise on the bias states, but that change is not a focus of this 
chapter. The measurement noise covariance depends on the sensors used; like 
the other measurement noise covariance shown in the chapter, it is diagonal 
and made up of the individual sensor noise variances. 
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Figure 5.11 Sideslip estimate during the asphalt experiment using the linear model-based 
estimator (case 4). 


Figure 5.11 shows the results of the sideslip estimation using the lin- 
ear model-based estimator with the two antenna GPS measurements and a 
yaw rate gyro (Case 4). As seen in the figure, the linear estimator does a 
good job of estimating the sideslip around the turn of the track, even in the 
presence of the 8° road bank, and during the first part of the lane change 
and slalom maneuvers. However, as would be expected with a linear esti- 
mator, it does not perform adequately during the more extreme maneuvers 
when the vehicle begins to operate in the nonlinear region of the tire. 


5.8.2 Nonlinear Tire Model 


The model-based estimator can be improved by using the Dugoff (or other 
nonlinear) tire model [22-24] to more accurately capture the nonlinear ef- 
fects of the tire. The full nonlinear dynamic model given in (3.8) and (3.9), 
in conjunction with the Dugoff tire model given in (3.45) through (3.48), 


Vehicle Dynamic Estimation Using GPS 165 


are used in the state estimation continuous propagation step of the estimator, 
(A.92) (ie., to calculate f’). For the remainder of the estimator steps, instead 
of computing the Jacobian of the full nonlinear model, (A.8), the state transi- 
tion matrix is approximated. For each tire, the slope of the Dugoff model is 
computed at the current slip angle (a,). These slopes become the cornering 
stiffnesses in the state transition matrix; see (3.18). The slope of the Dugoff 
tire model is 
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Figure 5.12 shows the slalom portion of the asphalt run compared to 
the simulated vehicle model using the Dugoff tire model. The peak force 
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Figure 5.12 Sideslip and yaw rate during slalom section of the asphalt experiment. 
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Figure 5.13 Sideslip estimate (case 6) during asphalt experiment using model estimation 
with only yaw rate and lateral acceleration measurements. 


and tire cornering stiffness are taken from the nonlinear tire curve estima- 
tion shown in Figure 5.8. In simulation, the vehicle model with the Dugoff 
tire parameters is able to capture the initial slide of the vehicle (as seen in 
the sideslip and yaw rate plots), but becomes unstable at the end of the run. 
However, as seen in Figure 5.13 and Figure 5.14, the nonlinear model-based 
estimator is still able to accurately estimate the states through the entire ma- 
neuver. Figure 5.13 shows the estimation results using only the yaw rate and 
lateral acceleration measurements (case 6), while Figure 5.14 shows the esti- 
mation results with the addition of the two antenna GPS measurements (case 
4). Note the improvement of the sideslip estimates with the addition of the 
GPS measurements. 

To test the robustness of the model-based estimator, it is run with in- 
correct vehicle parameters. The vehicle parameters are chosen such that the 
incorrect vehicle model has the same understeer gradient, (3.33), as the G35 
[since vehicle understeer is fairly easy to estimate online; as seen in Figure 
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Figure 5.14 Sideslip estimate (case 4) during asphalt experiment using the two-antenna 
GPS system. 


5.15, the model estimator (case 4) with the incorrect model is able to ac- 
curately estimate the yaw rate through the turn, lane change] and part of the 
slalom. However, as seen in the figure, the sideslip estimation error is quite 
significant. Figure 5.16 shows the measurement residuals (€,) from the Kal- 
man filter estimator as described in the Appendix, (A.89). Note that the yaw 
rate measurement residual appears to be fairly random and centered about 
zero. However, inspection of the GPS measurement residuals provides a clear 
indication that there are errors in the Kalman filter model. This shows the 
benefit of the GPS measurements (from one- or two-antenna systems) to 
provide a level of confidence in the estimated states and the model associated 
with the estimator. Additionally, these figures demonstrate the importance of 
accurate knowledge of the front and rear tire parameters and not simply the 
understeer gradient of the vehicle. 

Finally, the nonlinear model-based estimator is tested on the low-u 
gravel surface described previously. Figure 5.17 shows the results of the 
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Figure 5.15 Sideslip estimate during a series of maneuvers using an incorrect model 
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Figure 5.16 Measurement residuals during a series 
model (case 4). 
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Figure 5.17 Sideslip estimate (case 4) during a hard right turn on a low-y surface using 
incorrect Dugoff tire parameters. 


nonlinear model-based estimator (case 4) using the Dugoff tire parameters 
from the asphalt experiment. Notice that although the estimator provides a 
fairly good estimate of yaw rate during this experiment, it does not capture 
the large sideslip from the vehicle slide on the low- surface. This demon- 
strates the importance of accurate peak tire force in the model based estima- 
tors. Figure 5.18 shows the sideslip estimation (cases 4 and 6) results using 
the correct Dugoff low- tire parameters, estimated in Figure 5.10. The 
estimation results with only inertial sensor measurements (yaw rate and lat- 
eral accelerometer) compared to the results with the two antenna GPS mea- 
surements are shown. Note that even with the correct model parameters, 
the yaw rate gyroscope and lateral accelerometer are unable to estimate the 
large slip angle produced in this maneuver. However, the addition of the 
GPS measurements allows the model based estimator to accurately capture 
the sideslip angle. 
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Figure 5.18 Sideslip estimate (cases 4 and 6) during a hard right turn on a low-u surface 
using the correct Dugoff tire parameters. 


5.8.3 Estimator Accuracies 


To show the benefit of identifying the tire parameters and reestimating 
the vehicle states with a model-based estimator, a simple covariance analy- 
sis can be performed to determine the accuracy of the various estimation 
algorithms [11]. A summary of the covariance analysis (given in terms of 
the 1-0 values) is shown in Table 5.2. It is important to note that the 
covariance values for the kinematic Kalman filter assume there are no un- 
modeled sensor dynamics in the system (since the kinematic Kalman filter 
only depends on integration of sensor measurements). Also, note that the 
accuracy of these estimates will improve with speed as seen in (1.9). To 
improve the overall accuracy of the estimates, more comprehensive kine- 
matic relationships could be added to reduce errors induced by roll and 
pitch. 
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Table 5.2 
Covariance Analysis Summary 


Two-Antenna Model-Based 
Estimated State (Sensors Used) (KKF) Kalman Filter 
Vehicle Heading (GPS only) 0.4° 0.03° 
Vehicle Heading (GPS and yaw gyro) 0.1° 0.01° 
Vehicle Course (GPS only) 0.36° 0.36° 
Vehicle Slip Angle (GPS only) 0.54° 0.25° 
Vehicle Slip Angle (GPS and yaw gyro) 0.37° 0.2° 
Vehicle Slip Angle 0.28° 0.05° 
(GPS, yaw gyro, and lat accel) 


Max 1-o Values) at 8 m/s. 


The covariance values given for the model-based Kalman filter assume 
there are no unmodeled vehicle or sensor dynamics in the system (since the 
model-based Kalman filter depends on the integration of sensor measure- 
ments with the vehicle model). The analysis given in Table 5.2 is dependent 
primarily on the INS sensors, such as sensor noise and bias stability, and not 
the vehicle model. Also note that while it is not discussed here, the model 
based estimator provides better estimates in the presence of gyroscopic scale 
factor errors (if the correct vehicle parameters are used) or in the presence of 


banked roads [25, 26]. 


5.9 Conclusions 


This chapter has shown the ability of a variety of estimators (utilizing GPS 
and inertial measurements) to provide accurate estimates of vehicle states for 
use in vehicle control systems. A method for using these measurements to 
obtain Dugoff tire parameters was provided. The method was tested using 
experimental data on high and low friction surfaces to verify the algorithms 
were robust to parameters changing with ground conditions. A covariance 
analysis provided the theoretically achievable accuracy, and results showed 
an improvement when using the estimates from the model based estimator 
over a kinematic Kalman filter. These improved state estimates can be used 
to develop a more reliable and robust ESC system. Although the model based 
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estimator can provide vehicle information during a GPS outage, the kine- 
matic Kalman filter can only provide reliable sideslip information for short 
periods without GPS measurements. Future work will investigate how this 
impacts the model-based Kalman filter as well as other methods to improve 
sideslip estimation. 
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GNSS Control of Ground Vehicles 
David W. Hodo 


6.1 Introduction 


Several different methods of vehicle control are commonly used for autono- 
mous vehicles. In this chapter, vehicle models presented in Chapter 3 are 
used. to develop path-following controllers using both classical and modern 
(state-space) methods. Control methods are presented for two different types 
of paths. The first path type is composed of discrete locations or waypoints 
for the vehicle to drive to. The second uses a continuous function to define 
the path. A controller is developed to follow continuous paths consisting of 
lines and circular arcs, but the method is easily extensible to any path defined 
by a closed-form, continuous function. Finally a model for a vehicle towing a 
trailer or implement (e.g., farm tractor, 18-wheeler, and so forth) is given and 
a controller is developed for path following of the trailer. 


6.2 Vehicle Model 


An accurate vehicle model is required before a vehicle can be controlled. The 
majority of autonomous vehicles fall into two categories: Ackermann steered 
and differential drive. An Ackermann-steered vehicle is one with steerable 
front wheels such as a standard passenger car or truck. A differential drive 
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vehicle is one with nonsteerable wheels or tracks, such as a tank, bulldozer, or 
many small robotics platforms. The inputs for an Ackermann-steered vehicle 
are typically acceleration and a steering wheel input. The inputs for a dif- 
ferential drive vehicle are typically wheel velocity commands or wheel motor 
voltages. 

Models can be derived for both vehicle types between the vehicle’s in- 
puts and the vehicle’s forward velocity and yaw rate. Once a model between 
the inputs, velocity, and yaw rate is found, the differences in the vehicle types 
can be ignored. Several vehicle models for both categories are developed in 
Chapter 3. The methods discussed in this chapter can be used with any of 
the vehicle models previously presented. For demonstration purposes, a four- 
wheeled, Ackermann-steered vehicle is assumed. 

A basic vehicle diagram for an Ackermann steered vehicle is shown in 
Figure 6.1. The vehicle’s center of gravity (CG) is denoted by the black circle 
in the center of the vehicle. The distances between the front tire’s contact 
patch and the CG and the distance from the CG to the rear tire’s contact 
patch are given by the distances a and 4, respectively. The angle between the 
direction the vehicle is pointing and the front wheels are pointing (steering 
angle) is denoted by the angle 6. The vehicle’s heading (the direction the 
vehicle is pointed) is denoted by the angle yand is measured clockwise from 
due north. The vehicle’s yaw rate is given by the angular velocity 7. The lon- 
gitudinal velocity is given by V, and the lateral velocity by V,,. 


N 


Figure 6.1 Vehicle diagram. 
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The vehicle’s throttle, brake, and steering wheel are assumed to be actu- 
ated using DC motors with the input being the voltage applied to the mo- 
tor (although the model used is suitable for many other actuation methods). 
Assuming the inductance of the motor’s windings is sufficiently small, the 
model of the motor between input voltage and rotational speed can be ap- 
proximated by a first-order system with a time constant T determined by the 
inertia of the motor’s load and a DC gain K, which is assumed to be 1 in the 
following examples [1]. 

The steering system is therefore modeled using a first-order lag and an 
integrator between the steering input ~ and the steer angle 6. A block dia- 
gram of this system including both the steering system and vehicle model is 
shown in Figure 6.2. The first block in the diagram from w to 6 represents 
the dynamics of the steering actuator. The integrator defines the relationship 
between steering wheel angular speed and steer angle. The final block defines 
the vehicle yaw dynamics. 

A second-order bicycle model (as presented in Chapter 3) is assumed 
for the model between steer angle 6and yaw rate r. The bicycle model is given 
in state space form in (3.12) and in transfer function form in (3.19). The fol- 
lowing substitutions are made for brevity and will be used in the remainder 
of the chapter: 


_ aCaf 
as 

(a +b)CopCar 
mV 1, 

_ Col, +mC2 

7 mV,1, 

_ CoCo - CGmV2 - CP 
7 mVelz 


nm 


n0 
(6.1) 
d 


do 


ul 1 d| 16 mstm |r 
Ty S41 s s*+ d,S+ dy 


Figure 6.2 Vehicle model block diagram. 
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where 
Gy Op Ag 
Cy =aCag - Cer (6.2) 
Cy =a’Cap +b’Cor 


Two control design methods are presented: one based on cascading clas- 
sical controllers and one using modern, state-space techniques. The classical 
method relies on models given as transfer functions, such as the model be- 
tween input and yaw rate given in (3.19). The modern control method relies 
on models given in state space form [2]. The model between input and yaw 
rate shown in Figure 6.2 can be converted into state-space form resulting in: 


Qru 64, An B 0 ueru 60 yu 
yu é ug,U &, U 
eeu Ar Bo 0 70,4 ° Uy, (6.3) 
2° é0 0 0 1 Uuédu €o U 
e.u é ye.u é@ yu 
Aig 80 0 0. -l/t5h86q A! Tati 
& 0 0 ougrt 
Uu L 
ya? 1 0 Ova ¢ (6.4) 
@ 0 1 OyEed t 
boo 0 ist 
where 
Co 
Ayan 
ny 
Cy 
Ay = Vz 
' mV. 
a (6.5) 
Ay =- 2 
TV 
Ci 
B= af 
m 
Capa 
By = 0f* 
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Table 6.1 

Infiniti G35 Vehicle Parameters 
Symbol Description Value 
m Mass 1,528.2 kg 
a Distance from CG to front tire 1.368m 
Db Distance from CG to rear tire 1.482m 
L Wheelbase = a + b 2.85m 
l Yaw moment of inertia 2,400 kg*m? 
Cort Front tire cornering stiffness 91,674 N/rad 
Cost Rear tire cornering stiffness 152,788 N/rad 
Omox Maximum steer angle 0.8 rad 
Ox: Maximum steer angle rate 0.85 rad/sec 
Ts Steering system time constant 0.1053 second 


The classical techniques only require knowledge of the system output. 
When using state-space techniques, however, full state feedback is used; thus, 
the value of each state must be measured or estimated. All of the states used 
in this chapter can be measured or estimated using standard full-state estima- 
tion algorithms [2]. In the following sections, knowledge of all states will be 
assumed and no distinction will be made between actual state values and state 
estimates. 

Simulation results are shown throughout the chapter. The parameters 
used in the simulations are for an Infiniti G35 sedan and are given in Table 
6.1. Methods for identifying these parameters are discussed in Chapter 5. 


6.3 Speed Controller 


The vehicle model given above is an example of a multi-input, multioutput 
system. In order to simplify the design, however, speed and steering are con- 
trolled separately. A controller is designed to control the vehicle’s forward 
velocity V,. Then, for the purpose of steering control, the longitudinal veloc- 
ity is considered constant. 

A desired speed is selected for each segment of the vehicle’s path. 
The speed controller uses this desired speed as well as the vehicle’s current 
speed to set the throttle and brake positions. The model given in (3.30) is 
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linearized to produce a simple first-order model described by a DC gain and 
time constant between input and velocity. A second-order model could be 
used to capture the dynamics of the DC motor used to actuate the throttle 
and brakes as well, but experience has shown that these models have time 
constants considerably faster than those of the vehicle dynamics and can 
be safely ignored. The model between the throttle input u, and velocity is 
given by (6.6) and the model between braking input w, and velocity is given 
by (6.7). 


Ve. K, 
Gp;(s) == = : 


6.6 
u, Ts +1 0-8) 
Vy, Z Ky 
Pols) up Ts +1 


Although the same forms are used, the parameters are different in the 
two models due to the difference in the way throttle and brake affect torque 
at the wheels. 

Two separate proportional-integral (PI) controllers are used to control 
the brakes and throttle. A feedforward term wis also included for the throt- 
tle input. The feedforward input is given in (6.8) where DC,,is the DC gain 
of the throttle plant given in (6.6). 


Udes 
= (6.8) 
is D Cp; 


Although a feedforward term is used, an integral term in the control- 
ler is still useful for accounting for uncertainty in the DC gain of the model 
and accounting for constant disturbances (e.g., road grade). The form of the 
feedback controller is the same for both the throttle and brake and is given in 
(6.9) where V,,,.= V,,— V. 


Ce en se (6.9) 
Verr & 5 Q@ 


Since applying both brake and throttle simultaneously is undesirable, 
control logic is needed to determine when the controllers are active. In order 
to prevent braking for speeds that are only slightly over the desired speed, 
the brake controller is only active when the speed error is negative and has a 
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value that is 20% or more of the desired speed. If this condition is not met, 
the throttle controller is activated. 


6.4 Vehicle Steering Control 


All methods of vehicle control discussed in this chapter require the yaw rate 
of the vehicle to be controlled. Controlling the yaw rate of an Ackermann- 
steered vehicle requires control of the vehicle’s steering angle. The left and 
right wheel speeds must be controlled to control the yaw rate for a differential 
drive vehicle. The relationship between wheel speeds and yaw rate is discussed 
in Section 3.6.1. Using state space techniques, the design of these controllers 
is incorporated into the designs of the heading and lateral position controllers 
that are discussed later in the chapter. When using classical design techniques, 
however, a cascaded control architecture is used, and separate, decoupled 
controllers control the steer angle 6 and the yaw rate r of the vehicle. 


6.4.1 Classical Steer Angle Controller 


The complexity and type of controller used for the steering system depends 
on the type of vehicle and actuators used. The input to a typical steering 
actuator controls the rate of change of the steer angle, rather than the steer 
angle itself (e.g., a DC motor driving the steering shaft). As discussed previ- 
ously, a first-order lag with time constant Ts; and a DC gain of 1 is assumed 
between the input and steering rate. The steer angle can then be determined 
by integrating the steering angle rate. The resulting model between steering 
input and steer angle is given by: 


Gp5(s) Tah eee (6.10) 


Nonlinearities in the steering mechanism such as deadband and satura- 
tions must be addressed separately from the baseline design, and for simplicity 
are not discussed here. Since the plant is type 1 (one open-loop integrator), 
a proportional-derivative (PD) controller Ges can be used to control the steer 
angle of the vehicle with zero steady-state error to a step input [1]. The form 
of the controller is given in (6.11). 


Gcs(s) =K ps (1 +K pss) (6.11) 
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The closed-loop dynamics of the steering system can then be calculated 
and are given in (6.12). 


K 
ED a pas 


= 76 
Gscx(s) = : (6.12) 
: 2 e+KpsK ps0 , Kps 
S ts¢ at 


TS G@ Ts; 


A desired closed-loop characteristic is determined and standard pole 
placements techniques are used to calculate the proportional and deriva- 
tive gains. Since additional controllers are to be added around this inner 
loop, a closed-width bandwidth is needed so that the bandwidth of the 
outer loop can be set to approximately 1 Hz and the inner-loop dynamics 
can still be ignored. A closed-loop bandwidth of approximately 20 Hz is 
chosen with a 0.707 damping ratio. Using root locus techniques results 
in gains of Kps = 787 and Kp; = 0.01 when using the parameters given in 
Table 6.1. 


6.4.2 Classical Yaw Rate Controller 


Next, a yaw rate controller is developed using the same process. A second- 
order, bicycle model between the steering angle and yaw rate is used as an 
example: 


ns +no 


(6.13) 
+d\s +d 


Gp,(s) = = 


A PI controller is used to control the yaw rate. Since the plant is type 0, 
an integral term is needed in the controller to prevent steady-state error [1]. 
The form of the controller is given by (6.14). 


Go.(s) =Kp, a+ br So () (6.14) 


The closed-loop system response can be sped up by including a feed- 
forward control term. The feedforward term sets the predicted steady-state 
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Figure 6.3 Closed-loop yaw rate block diagram. 


steer angle so that only disturbances and errors in the model must be ac- 
counted for by the feedback controller. The desired steer angle is calculated 
using the DC gain of the plant given in (6.13). For low speeds, the DC 
gain can be approximated as Vy/Z using the kinematic relationship given 


in (3.5). 


"des Lies 
DC, » V, (6.15) 
A block diagram of the closed-loop steering and yaw rate systems is 
shown in Figure 6.3. When designing the yaw rate controller, the closed- 
loop bandwidth is chosen to be 5—10 times slower than the bandwidth of the 
closed-loop steer angle. This means that the dynamics of the steering system 
can be ignored when determining the closed-loop yaw dynamics and only the 
DC gain DCs must be accounted for. 

Again, the yaw rate gains can be determined using pole placement tech- 
niques. A closed-loop bandwidth of approximately 4 Hz with a damping ra- 
tio of 0.707 is chosen. The controller is designed using root locus techniques. 
For a forward velocity of 4 m/s, these closed-loop characteristics are achieved 
with gains of Kp, = 0.11 and K;, = 126.36, resulting in a third pole located at 
s=—39. 

Designing using root locus or other graphical techniques poses some 
practical challenges for the yaw rate system. The parameters of the bicycle 
model change with speed. A Bode plot of the model given in (6.13) for four 
speeds is shown in Figure 6.4. In order to maintain the same steering dynam- 
ics regardless of speed, the gains must also change with speed. One solution 
to this problem is to calculate the gains for a chosen set of speeds over the 
possible range and either linearly interpolate between speeds or generate a 
polynomial curve fit to calculate gains between the speeds. 

An alternative (and simpler) approach is to calculate the gains using a 
desired characteristic equation CE. The form of a desired CE consisting of a 
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Figure 6.4 Bicycle model Bode plot for various speeds. 


second-order system with damping ratio ¢, natural frequency @,, and a third, 
real pole p; is given in (6.16). 


CE, prs (s) =(,2 +2¢60,,5 +002) (s +73) 
(6.16) 
= +26 +p3)s° + (0 +260 ps) + p30, 


The closed-loop characteristic equation for the vehicle’s yaw rate with 
a PI controller is given by: 


CE,cr(s) =8° +(d +DC3K pm) s” 
(6.17) 
+(DC5K pnp +DC5mK j, +dy)s +DCsnoK 1, 


The gains and third pole location are calculated by equating coefficients 
in the two equations. This provides an equation for the gains in terms of the 
model parameters and desired characteristics allowing them to be recalculated 
as the parameters change. Since only two gains are used, only two poles can 
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be placed. The third pole’s location must be checked for stability once the 
gains are calculated. 


6.5 Waypoint Control 


A common method of defining a desired path for a vehicle is to define a list of 
evenly spaced locations or waypoints for the vehicle to drive through [3-5]. 
In the following section, a method for controlling a vehicle to follow a series 
of waypoints is presented. The method involves controlling the heading w 
of the vehicle to drive it towards the given waypoint. A model for determin- 
ing the heading of the vehicle is presented, and controllers are designed us- 
ing both classical and state-space methods to control the vehicle to a desired 
heading. Finally, a method for determining a desired heading from a given 
set of waypoints is presented. 


6.5.1 Heading Model 


Once a model between the vehicle input w and yaw rate r has been found, 
the vehicle’s heading y can be found by integrating the yaw rate. For the 
classical, cascaded approach, an additional block representing an integrator is 
added to the model. The resulting block diagram is shown in Figure 6.5. 

For the modern control approach, the state-space model given previ- 
ously is updated to include the vehicle’s heading with the addition of a fifth 
heading state, the derivative of which is yaw rate. The updated state-space 
model is given in (6.18) and (6.19). 


eyu @ 1 0 0 0 uwayu 60 t 
é.d é ue ué, U 
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Figure 6.5 Heading model block diagram. 
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6.5.2 Heading Error Calculations 


In order to control a vehicle to the desired path, a reference heading must 
be calculated. A list of waypoints consisting of positions and speeds are 
provided to the controller by a path planner. Before a reference heading 
can be determined, a single waypoint must be chosen to drive to. Choosing 
a waypoint requires knowledge of the vehicle’s current position (¢, 7) and 
heading. 

The waypoint chosen must meet two conditions, illustrated in Fig- 
ure 6.6 [6]. First, it has to be in front of the vehicle. This is determined 
by drawing a line perpendicular to the heading of the vehicle and ignor- 
ing any waypoints behind the line. Waypoints that fail this condition are 
shown with thin, dashed Xs in Figure 6.6. Second, the chosen waypoint 
must be at least a given distance (referred to as the look-ahead distance, 


(ges des) 


Figure 6.6 Waypoint selection. 
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Dy,4) away from the vehicle. Waypoints that pass the first test, but fail this 
test are shown with thicker, dashed Xs. The closest waypoint that meets 
both of the given conditions (¢, 24) is chosen and is shown with a thick 
solid X. 

The choice of the look-ahead distance substantially affects the nature 
of the reference path. Choosing a look-ahead distance that is too short can 
cause a waypoint that is almost directly beside the vehicle to be chosen, re- 
sulting in aggressive steering and driving perpendicular to the path, rather 
than smoothly merging onto it. Choosing a look-ahead distance that is too 
long causes the vehicle to choose the next waypoint long before the currently 
chosen waypoint has been reached, resulting in the vehicle cutting corners 
during turns. It is common to vary the look-ahead distance with speed to 
make the steering smoother by adding damping to the heading controller at 
higher speeds. This is similar to the method used by human drivers where at 
low speeds, focus is near the front of the vehicle; as speed increases, however, 
it becomes necessary to look further ahead. The look-ahead distance used in 
the simulations that follow is given in (6.34). 

Once the waypoint is chosen, a reference heading and heading error is 
computed. The reference or desired heading W,, is the angle of a line drawn 
between the vehicle’s position and the chosen waypoint. The desired heading 
is measured clockwise with respect to north and is given by: 


Woes =tan’! ati =e : (6.20) 
e-n 


The heading error is defined as the angle between the vehicle heading 
and the reference heading as given in (6.21). 


Werr =W- Waes (6.21) 


In the following section, a controller is developed to drive the vehicle 
toward the waypoint by driving this error to zero. 


6.5.3 Heading Control 


Once a model between the steering input and heading and a desired reference 
value have been obtained, a controller is needed to drive the vehicle’s heading 
to the reference value. Both classical and modern methods are described. 


188 GNSS for Vehicle Control 


6.5.3.1 Classical Method 


When using the classical method, a third feedback loop is closed around 
the model between steering input and heading. As discussed previously, the 
transfer function between the yaw rate and heading consists of a pure integra- 
tor. A transfer function for the heading plant is given in (6.22). 


Gpy(s) = = (6.22) 


Since the system is type 1, zero steady-state error can be achieved using 
a PD controller to control the vehicle’s heading. The form of the controller 
is given by (6.23). 


Gcy(s) =Kpy(1 +K pys) (6.23) 


A block diagram of the closed-loop system including the steering, yaw 
rate, and heading models is shown in Figure 6.7. 

If the closed-loop dynamics of the vehicle’s heading are chosen to be 
sufficiently slower than the closed-loop yaw rate dynamics, the yaw rate dy- 
namics can be ignored and only the DC gain (DC,) of the yaw rate system 
must be included. The closed-loop dynamics of the vehicle’s heading in terms 
of the controller gains are given in (6.24). For a velocity of 4 m/s, a closed- 
loop response with a bandwidth of approximately 0.9 Hz is desired so the 1, 
real pole is placed at - 5.62. This is obtained by choosing the gains to be Kp, = 
12.8 and Kp, = 0.1. 


GwcL (s) = (6.24) 


6.5.3.2 State Space Method 


A state feedback controller can be developed by rewriting the state-space 
model previously given in (6.18) in terms of a heading error. This is done by 


Figure 6.7 Heading closed-loop block diagram. 
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making the assumption that the dynamics of the heading error are equivalent 
to the dynamics of the vehicle’s heading. This is true as long as the desired 
heading is constant. This assumption is not valid for the period immediately 
after a new waypoint is selected, which could possibly result in transient be- 
havior as each new waypoint is selected, but is otherwise valid. 

The dynamics of the heading error can be found by taking the deriva- 
tive of (6.21) resulting in: 


Werr = - Waes (6.25) 


Using the assumption that the desired heading is constant, (6.25) can 
be simplified to: 


Wor =W (6.26) 


The state-space model can now be rewritten in terms of heading error: 
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A full state feedback control law of the form 
u= Kx (6.29) 


where 


x=Wer r Vy 6 é]! (6.30) 
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is used to drive the heading error calculated in Section 6.5.2 to 0. The refer- 
ence or desired values for the remaining states are 0. The control gains K are 
calculated by solving the Riccati equation, resulting in a controller known 
as a linear quadratic regulator (LQR) [7]. The LQR control algorithm is an 
optimal control algorithm that optimizes the cost function given in (6.31) 
subject to the control feedback constraint given in (6.29). 


J =A x Qux tu" Ryu (6.31) 


The resulting controller minimizes a combination of the states weighted 
by the matrix Q, and the input(s) weighted by the scalar (or matrix) R,. Ex- 
ample weighting matrices are given in (6.32) and (6.33). These values are 
found experimentally by “hand tuning” until satisfactory performance is ob- 
tained [8]. 


Q, =diaglQy Q Q@ Q5 Qs] 
=diagll5 0 0 1 O 


(6.32) 


R, =0.1 (6.33) 


Since we are concerned with controlling the heading error, Q,, was set 
to 15 to penalize that state. Additionally, Qs was set to 1 to penalize the steer 
angle. Reducing the amount of steer angle adds more damping to the system. 
It is important to note that only the relative values of the LQR weighting 
matrices are important. The weighting matrices can be normalized with re- 
spect to any variable or state. If they are normalized to give a cost function 
that is unitless, then the units of each weighting value are the reciprocal of the 
square of the units of the weighted state. 


6.5.4 Simulation Results 


The vehicle is simulated following the path shown in Figure 6.8 consist- 
ing of waypoints spaced at 2-m intervals. These waypoints are shown with 
Xs in the figure. The path followed by the vehicle is shown with a solid 
line. The vehicle is started at a position 6-m north of the first waypoint. 
A constant speed of 4 m/s is used in the simulation. Two zoomed-in sec- 
tions are shown on the plot showing the vehicle as it converges to the 
path and during a sharp turn. A plot of the heading error verses time is 
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Figure 6.8 Waypoint following simulation path. 


shown in Figure 6.9. The errors are low on the straighter sections of the 


path where the desired heading remains fairly constant, but increase in 
turns. 
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Figure 6.9 Waypoint following simulation heading error. 
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The look-ahead distance used in the simulation is given in (6.34), where 
D nin is a minimum look-ahead distance and D,,,;, is a scale factor that adjusts 
the look-ahead distance with speed [6]. For the previous simulation, both 
Dyin Nd Dia are set to 1. 


scale 


Dra = sealeV x +Dynin (6.34) 


As discussed in Section 6.5.2, one of the drawbacks of waypoint control is 
the possibility of cutting corners at higher speeds or with longer look-ahead dis- 
tances. An example of this is seen in the following simulation, where the vehicle 
speed is increased to 8 m/s and the look-ahead scale factor D,,,;, is increased. to 
1.5. The corner shown in the zoomed section in the bottom right of Figure 
6.8 is shown in Figure 6.10. The result of the higher speed and look-ahead 
distance is that the vehicle chose a new waypoint significantly before reaching 
the previous waypoint, resulting in the corner being cut short. One possible 
solution to this problem is to include a lateral distance measurement from the 
path in the controller. This solution is explored in the following section. 


6.6 Lateral Control 


Another option for defining a desired path for a vehicle is a path defined by 
a continuous curve [9-11]. Paths of this type can be defined using many dif- 
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Figure 6.10 Effect of look-ahead distance on waypoint control. 
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ferent types of continuous functions [12]. A simple form of continuous path 
function consisting of line segments and circular arcs with a constant radius 
will be considered in this section. These paths are commonly referred to as 
Dubins’s paths after L. E. Dubins who showed that the time-optimal (ne- 
glecting vehicle dynamics), continuously differentiable path between any two 
points in 2D space consists of lines and circular arcs of a minimum radius 
[13]. The disadvantage to this type of path is that a discontinuity in curvature 
exists at the intersection of line and arc segments [14]. As can be seen from 
(3.2) this equates to a discontinuity in the desired steering angle resulting in 
transient behavior at these intersections. Modifications involving clothoids or 
splines can be used to eliminate the problem, but result in suboptimal paths 
[14-16]. Also, paths defined using clothoids do not have a closed form expres- 
sion and require numerical integration techniques to calculate lateral errors. 

When following a path defined by a continuous curve, both the lateral 
position of the vehicle as well as its heading are controlled. The methods 
described are easily extensible to paths defined by other types of continuous 
curves. The control design is not dependent on the type of path. Only the 
error calculation methods need to be modified based on the type of path cho- 
sen. Lateral control is also useful in automated highway systems and other ap- 
plications where no mathematical path is defined. Lateral errors from a lane or 
corridor can be calculated using sensors such as magnetometers [17], LIDAR 
[18], or vision systems [19] and used to control the vehicle’s position. 


6.6.1. Error Calculation 


In the previous section, when controlling the vehicle along a series of way- 
points, a reference heading was determined and used to steer the vehicle to- 
ward a waypoint. When paths are defined by a continuous function, a lateral 
error as well as a heading error are calculated and used to steer the vehicle 
along the path. Although desired paths are typically more difficult to define 
using continuous functions, the addition of the lateral error measurement 
allows this type of path to be followed more precisely than a path defined by 
waypoints. 

Rather than choosing a waypoint to drive to, as was done in Section 
6.5.2, a reference point on the path must be chosen. The lateral distance 
between this point and a control point on the vehicle is calculated to provide 
a lateral error y,,, between the vehicle and the path. Controlling the vehicle’s 
lateral error forces the control point on the vehicle to be as close as possible 
to the path. 
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It is desirable not only for the vehicle’s control point to be close to the 
path, but also for the vehicle to be oriented so that it is pointed along the 
path. This is done by controlling the vehicle’s heading (as was done in way- 
point control) as well. A desired heading is found by calculating the angle of 
a line tangent to the curve at the reference point. The heading error can then 
be found as described previously in (6.21) by subtracting the desired heading 
from the vehicle’s actual heading. Controlling the vehicle’s heading results 
in the vehicle’s orientation always being tangent to the path and therefore 
pointed in the direction of the next reference point. 

Both the lateral and heading errors are illustrated in Figure 6.11. Meth- 
ods for calculating these errors (as well as reference values for the remaining 
states) for both lines and circular arcs are given in the following sections. 

The closest point on the path to the vehicle’s control point is chosen as 
the reference point. A look-ahead distance similar to the one used in waypoint 
selection can be used in selecting the reference point to add additional damp- 
ing to the system, but is not required. The need for using a look-ahead dis- 
tance increases with speed as the system’s open-loop damping decreases [6]. 


6.6.1.1. Error Calculation for Lines 


The lateral error between the vehicle and a path segment given by a line is 
defined as the distance of a line segment drawn perpendicular to the vehicle’s 
heading between the vehicle’s control point and the path segment. This is 
the distance labeled y,,,. in Figure 6.12. In order to simplify calculations, 
this distance is approximated by calculating the shortest distance between the 


Figure 6.11 Lateral position error and heading error from a continuously defined path. 
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(e, 1;) 


Figure 6.12 Error calculation for lines. 


vehicle and the line. This approximated distance (y,,,) is also shown in Figure 
6.12. The two values are equal if the heading error is 0. Since the heading 
error is also being controlled, this is a valid assumption once the vehicle is on 
the path. 

A simple expression for calculating the lateral error for a path consisting 
of a line segment between points (¢,, 7,) and (e, 73) is given in (6.35) [9]. 


‘ _(m - me He2 - e1:)n Hem) - (erxm)) 
Ve - 4)? Hm +m)’ 


(6.35) 


The desired heading is found by calculating the angle with respect to 
north made by the line segment. 


-1 2 - e 6 


m2- mg 


Waes =tan (6.36) 


The heading error can then be found by subtracting the reference head- 
ing from the vehicle’s actual heading. 


Werr =W- Waes (6.37) 
When following a straight line in steady state, the vehicle should not be 


turning. Therefore the reference yaw rate, steering angle, and steering angle 
rate are all set to 0. A summary of the error equations is given in Table 6.2. 


196 GNSS for Vehicle Control 
Table 6.2 
Error Equations for Line Segments 
Error Equation 
_((m- me He2 - e:)n Hem) - (e2m)) 
Lateral Yer = 5 5 
(e2- ei’ Hinz +m) 
2 - e10 
=y- tan! . 
Heading Vor =W mmo 
Yaw rate liget 
Lateral velocity Ver = Vy 
Steer angle Sore = 


Steer angle rate 


6.6.1.2 Error Calculation for Arcs 


Lateral and heading error calculation for circular arcs is similar to that of line 
segments. Again, small heading errors are assumed and the lateral error is 
calculated by finding the shortest distance between the vehicle and the arc. 
This is done by finding the distance between the vehicle’s position and the 
center of the arc and subtracting the radius of the arc [9]. The result is given 


in (6.38) and is illustrated in Figure 6.13. 
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Figure 6.13 Error calculation for arcs. 
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The desired heading can be found by calculating the angle of a line 
tangent to the arc at the reference point. A simple method of calculating the 
desired heading is to find the angle of the line segment between the center 
of the arc and the vehicle’s position. This line is perpendicular to the desired 
heading at the reference point. The desired heading in radians can then be 
found by adding 7/2 to the angle of the line if the arc is being followed in a 
clockwise direction or subtracting 7/2 from the angle if the arc is being fol- 
lowed in a counterclockwise direction. The equation for desired heading is 
given in (6.39) where cw = 1 for arcs followed in a clockwise direction and 
cw = —1 for arcs followed in a counterclockwise direction. 
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Unlike line segments, a steady-state yaw rate (and therefore steer angle 
as well) is required to follow an arc. When driving on an arc of constant ra- 
dius, the rate of change of the heading of the vehicle is equal to the angular 
velocity of the vehicle [11]. If input to the model was the yaw rate, this would 
be accounted for in a feedforward term in the controller. When the steering 
system dynamics are included, however, a reference yaw rate and steer angle 
are used. The reference yaw rate is given by: 


wis » (6.40) 


where R is the radius of the arc. A reference steer angle can then be deter- 
mined by looking at the model between steer angle and yaw rate given in 
(3.2), which assuming small steer angles and no side slip is given by: 


L 
) es . 
de R (6.41) 


where L is the vehicle wheelbase. When significant side slip is expected, the 
desired steer angle can be more accurately calculated by taking into account 
the DC gain of the bicycle model as shown in (6.15). 

Once the desired steer angle is reached, it is constant for the duration of 
the arc and so the desired steering rate is 0. A summary of the error equations 


is given in Table 6.3. 
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Table 6.3 
Error Equations for Arc Segments 


Error Equation | 


Lateral error Yer = (e- ec)? Hn- ne)? - R 


-1@-60 a, 6 


i =y- tan ate— CW 
Heading error Wer =yV &)- ie > F 
Vy 
Yaw rate error lerr = ~ R 
Lateral velocity error Ver =Vy 
L 
Steer angle error Oem =6 - FR 
Steer angle rate error Ser =, 


6.6.2 Lateral Position Model 


Once a heading and lateral error are determined, a model is needed to relate 
these errors to the steering input. A model for the vehicle’s heading was given 
previously in Section 6.5.1 when discussing waypoint control. A model of 
the vehicle’s lateral position is now needed. The dynamics of the lateral error 
from a line or curve of the vehicle are given by [8]: 


Jerr =Vysin(Werr) +(Vy = Lory) cos( Wer) (6.42) 


where Lp is the distance from the control point to the center of gravity of 
the vehicle. In general the longitudinal velocity Vy of the vehicle is much 
greater than the lateral velocity at the CG Vyand so the lateral velocities are 
neglected. This allows the model to be simplified to: 


Jerr =Vysin (Wer) = Lopy COS( Wey) (6.43) 


Other researchers, including [20], have used a kinematic model, which 
also assumes no lateral velocities due to sideslip. For a kinematic model of the 
vehicle, the point at the center of the rear axle will have no lateral velocity. 
Because the kinematic model assumes no tire side slip, the velocity vector must 
be along the direction of the tire. There is assumed to be no lateral velocity at 
the tires and therefore no lateral velocity of the vehicle between the rear tires. 
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It is important to note that the dynamics of the system change by mov- 
ing the control point (thereby changing Z;p) [21]. The system becomes a 
nonminimum phase (contains a zero in the right half plane) for a control 
point located behind the CG assuming the lateral velocity at the CG is 0. For 
this work, the control point is assumed to be at the CG of the vehicle. Set- 
ting the control point to be at the vehicle’s CG (Z¢p = 0) and assuming small 
heading errors, the model can be simplified to: 


Jerr =VyxWorr (6 44) 


The resulting block diagram between input and lateral position is 
shown in Figure 6.14. 

The state space model developed previously for heading control and 
given in (6.27) can be augmented to include the lateral position dynamics 
through the inclusion of a lateral error state. The resulting model is given in 


(6.45) and (6.46). 
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Figure 6.14 Lateral position model block diagram. 
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6.6.3 Lateral Position Control 


6.6.3.1 Classical Method 


When using the classical method, a third feedback loop is closed around the 
model between steering input and lateral position. To simplify the design, the 
model between yaw rate and heading used for heading control is combined 
with the lateral position model given in (6.44) to form a model between the 
yaw rate and lateral position. The transfer function for the lateral model is 


given in (6.47). 


err 1% 
Gols) = (6.47) 
S 


A PD controller is used to control the vehicle’s lateral position. Addi- 
tional damping is added to the system through the derivative term to account 
for the two poles at the origin in the model. The form of the controller is 
given by (6.48). 


Gg (s) =Kp, (l +K pys) (6.48) 


A block diagram of the closed loop system including the steering, yaw 
rate, and lateral position models is shown in Figure 6.15. When determining 
the lateral position closed-loop dynamics, the inner-loop yaw rate dynamics 
can be neglected since they are many times faster than the lateral position dy- 
namics. Only the DC gain of the inner-loop transfer function DC, is included 
in the closed-loop dynamics. The closed-loop lateral position dynamics, ne- 
glecting the yaw rate dynamics, are given in (6.49). For a forward velocity of 
4 m/s, setting Kp, = 8.385 and Kp, = 0.24 results in a closed-loop system with 
a bandwidth approximately 0.9 Hz and a damping ratio of 0.707. 
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Figure 6.15 Lateral position closed-loop block diagram. 
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6.6.3.2 State-Space Method 


A state feedback control law of the form 


u =- Kx (6.50) 


where 


x=Lyer Wer r Vy 6 OFF (6.51) 


is used to drive the lateral position error to 0. Since full-state feedback is used, 
it is assumed that a measurement or estimate of all states is available. The con- 
trol gains K are calculated to form a linear quadratic regulator as discussed in 
Section 6.5.3.2. Example weighting matrices are given in (6.52) and (6.53). 


Q. =diag [Q, Qy Q, Qp Qs Qs] (6.52) 
=diag {1 0 0 5 O 


R, =0.1 (6.53) 


Since we are concerned with controlling the lateral error, Q, was set to 
1 to penalize that state. Additionally, Qs was again set to 5 to penalize steer 
angle, which adds additional damping to the system. The selected control 
weighting values given above were found experimentally by hand tuning until 
satisfactory performance was obtained. Using the parameters given in Table 
6.1 and forward velocities between of 2 m/s and 8 m/s, the above weighting 
matrices result in the gains given in Table 6.4. 

The open-loop and closed-loop poles of the system are shown in Figure 
6.16 and Figure 6.17, respectively. Note that the bandwidth of the system 


Table 6.4 
Lateral Error Gains 
Velocity Gains 
2 m/s K=[3.16 17.85 0.267 0.019 18.28 1.76] 
4m/s K=([3.16 22.51 0.663 0.046 28.17 2.61] 
6 m/s K=[3.16 26.45 1.138 0.078 37.24 3.34] 
8 m/s K=[3.16 30.06 1.670 0.113 45.71 3.98] 
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Figure 6.16 Lateral position model open-loop poles. 


varies with velocity. It is also interesting to notice that only the integrator 
poles are altered from the open-loop system to the compensated closed-loop 
system. 
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Figure 6.17 Lateral position model closed-loop poles. 
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6.6.4 Simulation Results 


The vehicle is simulated with forward velocities of 2 m/s and 8 m/s along a 
line heading due north using the state-space controller developed in Section 
6.6.3.2. There is an initial offset of 2m from the path. The resulting lateral 
error response is shown in Figure 6.18. The settling time decreases at higher 
speeds, but the damping remains constant as is predicted from the closed- 
loop pole locations. 

The same straight line path is simulated again using two different sets 
of gains at a forward velocity of 2 m/s. The lateral error response is shown 
in Figure 6.19. For the first run, the Qs entry in the LQR weighting matrix 
is set to 5. In the second run it is set to 0. The additional damping added by 
penalizing steer angle is seen in the response. 


6.7 Implement/Trailer Control 


Previously, controllers were developed for autonomous path following of a 
point on a vehicle. In some applications (precision agriculture [8, 21], fac- 
tory automation [22], UXO/mine detection [9], and so forth), however, it 
is advantageous to control a point on a trailer or implement towed behind 
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Figure 6.18 Simulated line tracking at 2 m/s and 8 m/s. 
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Figure 6.19 Effect of weighting matrices on response. 


a vehicle. In the following section, a method is presented to place a control 
point on a trailer or implement towed behind a vehicle on a path. A model 
of the towed trailer is given and a state feedback controller is developed to 
control the trailer. 


6.7.1 Trailer Model 


A kinematic model is used to describe a trailer or implement towed behind 
the vehicle [8, 23]. A schematic of a vehicle pulling a trailer is shown in 
Chapter 3 in Figure 3.13. The trailer’s point of zero lateral velocity (ZLV), 
as well as its control point (cp), is shown in the figure. The control point 
defines the point on the trailer that should follow the specified path. This 
point can be chosen by the designer and is often chosen to be the location 
of the GPS antenna on the trailer if one is used to determine the trailer’s 
position. 

A kinematic model that relates the vehicle velocity Vy and yaw rate r to 
the angle between the vehicle and the trailer referred to as the hitch angle @ is 
given by (3.63) in Chapter 3. The position of the trailer’s control point can 
be related to the position of the vehicle using the geometry of the system and 
this hitch angle. The relationships between the positions, orientations, and 
velocities of the robot and trailer are given by [9]: 
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e, =e- Lysin(w)- Lc sin(y +9) (6.54) 
nm, =n- Ly cos()- Lc cos(w +9) (6.55) 
Wr =y +e (6.56) 

V, =Vx cos(p)- Ler sin(@) (6.57) 


where (e,, 7,) is the position of the trailer’s control point, y, is the heading 
of the trailer, V, is the velocity of the trailer, and L- is the distance from the 
hitch point to the control point. These equations can also be used to calculate 
the position of the trailer from the position of the vehicle and the hitch angle 
if a measurement of the hitch angle is available rather than a direct measure- 
ment of the trailer’s position [24]. 

The lateral dynamics of the trailer can be obtained by assuming the 
vehicle and trailer are tracking a line oriented due north and taking the de- 
rivative of the east position of the trailer given in (6.54). The resulting lateral 
dynamics are given by: 


in =e, =E- Ly cos(w)- Lc (Wwt+@)cos(w +¢) (6.58) 


Assuming small hitch angles and heading errors, this can be simplified 
to: 


VN =VXVW ory = LH Wer sal Le (Wer +9) (6.59) 


The hitch dynamics given in (3.60) can also be linearized assuming 
small angles resulting in: 


pee 0. Ve 


=r¢ ae a (6.60) 
T T 


The dynamics of the hitch angle (6.60) and implement lateral position 
(6.59) are placed in state-space form and added to the vehicle model given 
previously in (6.27). The resulting state-space model is given in (6.61) and 
(6.62). 
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6.7.2 Error Calculation 


Calculating the lateral error for trailer control uses the same methods given in 
Section 6.6.1 for vehicle lateral control with the exception that the position 
of the trailer is substituted for the position of the vehicle. The equations for 
the lateral error from a line and arc segment are given in (6.63) and (6.64), 
respectively, where (e, 7,) is the position of the trailer and R, is the radius of 
the path followed by the trailer. 


y,. _(m - mr)” e Her- a1)” m +e” m2)- (er° m)) 6.63) 


V(e2 - e1)? H(mz +m) 
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Yer = (er - ed” H(ny- nd” - R; (6.64) 


The desired heading, yaw rate, and steer angle can also be calculated 
using the methods given in Section 6.6.1, with a few minor modifications. 
The equations presented previously calculate desired values for the vehicle 
when it is following a given path. Now, however, the desired values for the 
vehicle are needed when the trailer is following a given path. When following 
line segments, the path followed by the vehicle and trailer are the same and 
the equations given previously hold. When the trailer tracks an arc of a given 
radius, however, the path followed by the vehicle and the trailer are not the 
same. The vehicle must follow a larger arc. For a desired trailer path with a 
radius R,, the radius of the arc followed by the vehicle is given by: 


R=, Ly? - Ly? +R? (6.65) 


This larger radius must be taken into account when calculating the de- 
sired yaw rate and steer angle of the vehicle. Once the radius of the vehicle’s 
path is known, the desired vehicle yaw rate and steer angle can be calculated 


using (6.66) and (6.67), respectively. 


Vx 
(ay =—_ 6.66 
tes = (6.66) 
L 
Odes > — 6.6 
di sae (6.67) 


A desired hitch angle @,,, is found by calculating the steady-state value 
of the hitch angle for both line and arc path segments. For a line segment, 
the trailer follows directly behind the vehicle and the desired hitch angle is 0. 
When following an arc, a constant angle exists between the vehicle and trailer 
and is given by: 


= Ly RR +1} +R? - Lily © 
4s R(L; +R?) : 
Paes =tan ¢ = (6.68) 
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The desired heading of the trailer can also be calculated using the meth- 
ods given in Section 6.6.1. The desired trailer heading Y,z,, is found using 
(6.36) for line segments and (6.39) for arc segments, again substituting the 
position of the trailer for that of the robot. The trailer model given previ- 
ously, however, is in terms of the vehicle’s heading. The desired vehicle head- 
ing can be found in terms of the desired trailer heading by rearranging the 
relation given in (6.56) resulting in: 


Waes =Wides - Plles (6.69) 


A summary of the error equations for lines is given in Table 6.5 and for 
arcs is given in Table 6.6. 


6.7.3 Trailer Control 


Once the errors from the path are determined, a controller is needed to drive 
the errors to 0. A full-state feedback control law is used to control the trailer’s 
lateral position (as opposed to the vehicle’s lateral position controlled in the 
previous section) [8]. The form of the controller is given by: 


u =- Kx (6.70) 
Table 6.5 
Error Equations for Line Segments for Trailer Control 
Error Equation 
_(m- mer Hee - ex)m Hern) - (e2m)) 
Lateral err = 5 7 
(€2 - €1)" Hn +m) 
Headi = ~ tan 12227 18 
eading Ver =W Sin - mo 
Hitch angle Perr =D 
Yaw rate ai 
Lateral velocity Ver =Vy 
Steer angle Sen = 5 
Steer angle rate San = 6 
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Table 6.6 
Error Equations for Arc Segments for Trailer Control 


Error Equation 


Lateral Ver =e - ec)? Hn - nc’ - Fi 


-1e@- & 0 a, 0 
&n- 1.8 & a 


Vehicle radius R=a/ ly? - Ly? +R? 


Hitch angle Pon = p- tan” (M,N) 
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— LyRR 4G Lp RP - Flr 
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Heading Wer =yw- tan 


_ > UulpR +R rl +A? - Rely 


r(G +A?) 
Y tl lor =I - Va 
aw rate err BR 

Lateral velocity Ver = Vy 

L 
Steer angle Serr =O - — 

R 
Steer angle rate Son = 8 

where 
$17 
x =r 9 War r 6 6 Ol (6.71) 


The control gains Kare again calculated to form an LQR as discussed in 
Section 6.5.3.2. Example weighting matrices are given in (6.72) and (6.73). 


Q, =diag[Q yt Qo Qy QR QW Qs Qs) 


6.72 
=diagIl5 0 0 0 0 1 J ore 


R, =0.1 (6.73) 
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Table 6.7 
Trailer Parameters 


Parameter Value 
Ly 1.6m 
Ly 3.5m 
le 3.6m 


Since we are concerned with controlling the trailer’s lateral error, Q,, was set 
to 15 to penalize that state. Additionally, Q;and QXwere set to 1 to penalize 
steer angle and add additional damping to the system. The selected control 
weighting values were found experimentally by hand tuning until satisfactory 
performance was obtained. 


6.7.4 Simulation Results 


The vehicle pulling a trailer is simulated following a line segment with an ini- 
tial offset of 2 meters and a forward velocity of 2 m/s. The parameters given 
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Figure 6.20 (a, b) Vehicle versus trailer controller comparison. 
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previously in Table 6.1 are used for the vehicle in the simulation. The trailer 
parameters are given in Table 6.7. 

In Figure 6.18(a) the control algorithm given in Section 6.6.3 is used 
to control the position of the vehicle, while Figure 6.20(b) has the feedback 
wrapped around the position of the trailer using the algorithm given in Sec- 
tion 6.7.3. As seen in the figure, by controlling the trailer, the vehicle over- 
shoots quite a bit in order to bring the trailer lateral error to zero almost 50% 
faster [and in a very similar fashion to the way the controller brings the lateral 
error of the tractor to zero in Figure 6.20(a)]. 

A simulation of the trailer along an arc segment is shown in Figure 6.21. 
The same parameters are used as the previous simulation with the vehicle’s 
velocity increased to 4 m/s. The vehicle and trailer start tracking a straight 
line near (Om, Om) and then follows a 15-meter radius arc centered at (40m, 
15m). Both the trailer and vehicle positions for the simulated experiment are 
shown in the figure, with the feedback loop closed around the trailer’s posi- 
tion. Note that the implement does not start tracking the arc at the tangent 
of the arc. Rather, the implement was tracking a line and then was asked to 
track the circle once it reached the edge of the arc. In order for the trailer to 
track the 15-m radius arc, the vehicle follows an arc of radius 15.4m. This 
discrepancy shows the importance of being able to control the trailer position 
on curved trajectories. 
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Figure 6.21 Trailer control along a line and arc. 
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Pseudolites for Vehicle Navigation 
Stewart Cobb 


As discussed in Section 1.1.2, navigation with GNSS satellite signals requires 
that several (generally four) satellite signals be visible to the navigating re- 
ceiver simultaneously, and the accuracy and reliability of GNSS navigation 
increases with each additional signal. Thus, GNSS navigation works well in 
outdoor environments with a clear view of much of the sky. 

Unfortunately, ground vehicles are often required to operate in environ- 
ments where much of the sky may be obscured by buildings, foliage, or ter- 
rain. Pseudolites (Section 1.2) have proved useful in such cases. This chapter 
examines several potential and actual applications of pseudolites, and several 
existing pseudolite systems, which have been deployed for such applications. 


7.1 Pseudolite Applications 


The most fruitful application of pseudolites to date has been in open-pit min- 
ing. Pseudolites have also been proposed to assist navigation in and around 
construction sites and through “urban canyons” formed by tall buildings 
in metropolitan areas. Some researchers have conducted experiments using 
groups of pseudolites indoors to enable GNSS-like navigation without access 
to any actual GNSS satellite signals. This section will discuss these actual and 
potential pseudolite applications. 
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7.1.1. Open-Pit Mining 


Miners have become accustomed to using GNSS technology [1, 2]. Geolo- 
gists use GNSS to plot the locations of surface samples and core samples, 
and then use the data to build a 3D map of the subsurface ore body. Mining 
engineers use the 3D map to plan their excavations efficiently. Surveyors use 
GNSS to lay out the excavation plan on the real-world topography. Blasting 
engineers define patterns of boreholes to be filled with explosives in order to 
fracture the proper amount of ore and rock, and drillers then use GNSS to 
place the holes precisely in the desired locations. 

After the explosives are detonated, excavator shovels move in to load 
the broken rock and ore into giant haul trucks. One haul truck may take the 
valuable ore to a crusher for further processing; another haul truck takes the 
ordinary rock to a dump pile. Each shovel-full of ore or rock must be loaded 
into the correct haul truck and sent to the correct destination, or the mine 
will lose money. Unfortunately, ore and rock often look the same. Rather 
than trying to discriminate between them by eye, the excavator operators use 
displays driven by GNSS and the 3D ore body map to decide whether a given 
shovel-full contains ore or rock and to place it in the correct haul truck. 

GNSS receivers mounted on the haul trucks allow a dispatcher to see 
the location of each truck in the mine and to direct the fleet of trucks most 
efficiently. Some mines are even beginning to experiment with robotic haul 
trucks, guided by GNSS and other sensors without a human driver. 

As we can see, GNSS technology has become essential to the efficient 
operation of open-pit mines. Unfortunately, many ore bodies extend deep 
beneath the surface. As the mine pit becomes deeper, following the ore body, 
the walls of the pit appear higher from the mine floor. The higher walls block 
more of the sky and more of the available GNSS satellite signals. Since the 
1990s, some mines have been shutting down operations during certain times 
of day because too few GNSS signals were available. These shutdowns can 
cost tens of thousands of dollars per hour in lost profit, but continuing to 
operate without good position data would cost even more [2]. 

Pseudolites provide an obvious solution to this problem. Pseudolites 
emplaced around the rim of the pit and at other appropriate locations can 
substitute for the GNSS satellite signals blocked by the walls of the pit, en- 
abling mine operations to continue uninterrupted (Figure 7.1). 

Beginning in 2000, Phelps-Dodge (now part of Freeport-MacMoran) 
installed a system of IN400 pseudolites from IntegriNautics (now Novariant) 
in their open-pit copper mine near Morenci, Arizona [3, 4]. This was the 
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Figure 7.1 Pseudolites replace GPS signals in an open-pit mine. 


first operational pseudolite system installed in a mine. The IN400 system is 
described in Section 7.2.1. 

Lessons learned from this installation contributed to the design of the 
follow-on Novariant Terralite XPS system. The Terralit XPS system was 
again first installed at Morenci, and has been in daily operational use since 
2005 [4-6]. Since then, the Terralite XPS system has been installed in over 
a dozen mine sites and is becoming a standard in the industry. The Terralite 
XPS system is described in Section 7.2.2. 

The Australian company Locata has also designed a system of pseudo- 
lites, known as LocataLites. Locata has performed several experiments with 
LocataLites in mines, but they have not yet reported that LocataLites have 
ever been installed for operational use in a mine. LocataLites are described in 
Section 7.2.3. 
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7.1.2. Construction Sites 


GNSS technology has become pervasive in construction. Architects create 
site plans based on GNSS mapping data. Surveyors use GNSS to lay out 
the construction site. Operators of earthmoving equipment use GNSS-driven 
real-time displays of the site plan inside their cab to determine where, when, 
and how much to dig. In some cases, the excavator shovel or bulldozer blade 
is automatically guided by a GNSS system following the site plan. 

All this GNSS technology works well in an open field with a clear view of 
the sky. Much construction work, however, takes place in built-up areas. If GNSS 
satellite signals are blocked by other buildings or vegetation in one direction, the 
PDOP (see Section 1.1.2) will be degraded, and the navigation accuracy with it. 
If satellite signals are blocked in too many directions, navigation will become im- 
possible. Again, pseudolites have been proposed as a solution to these problems. 
A pseudolite placed on or near each obstruction can restore the usefulness of 
GNSS navigation as if the construction were taking place in an open field. 

While pseudolites have been proposed for construction sites, there have 
been no published reports yet of even experiments in this area. Any of the 
pseudolite systems suitable for open-pit mining (see Section 7.1.1) should be 
suitable for construction sites as well. 


7.1.3 Urban Navigation 


In many urban areas, streets are surrounded by tall buildings, to the point 
that they have been called “urban canyons.” The buildings can block so many 
satellite signals that GNSS navigation becomes difficult or impossible at street 
level. Other techniques discussed in this book (inertial aiding, dead-reckoning, 
map-matching, and so forth) can be integrated into a navigation or guidance 
system, so that it is not wholly dependent on GNSS signals. Another approach, 
however, is to replace the blocked satellite signals with pseudolite signals. A 
pseudolite emplaced near the roof of a tall building, broadcasting down into 
the street below, can substitute for a satellite signal blocked by the building. 

In 2001 and 2002, the government of Japan funded several experiments 
to assess the feasibility of pseudolite-aided GNSS navigation in urban areas [7]. 
University researchers produced a prototype assistive device, which used GNSS 
and pseudolite signals, digitized maps, voice recognition, and voice synthesis 
to help blind pedestrians navigate city streets. These experiments used Integri- 
Nautics (now Novariant) IN400 pseudolites, and matching IN2400 receivers 
to augment GNSS navigation in the streets of downtown Tokyo (Figure 7.2). 
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Figure 7.2 Pseudolites installed for an experiment on a Tokyo street (GNSS Technologies 
Inc.). 


One can imagine a future in which pseudolites are widely distributed 
to help GNSS-equipped vehicles navigate through urban areas. If this future 
is to become real, three things must occur. A single standard for pseudolite 
signals must be defined and become widely accepted. The cost of equipping 
a street with pseudolites must become suitably small, perhaps comparable 
to the cost of equipping the same street with streetlights. Finally, the cost 
of adding pseudolite capability to a GNSS receiver must become negligible 
compared to the cost of the receiver itself. Given proper system design and 
economies of scale, these three goals may yet be achieved, and pseudolites 
may become an expected part of the urban infrastructure. 


7.1.4 Indoor Applications 


It is easy to imagine an “indoor GPS” system that uses pseudolites inside a 
building to replace the GPS satellite signals blocked by the building’s roof. 
This would be difficult in practice to accomplish [8]. It might work in a large 
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open space such as a gymnasium or concert hall, but it will not work well in a 
typical building containing corridors and rooms with low ceilings. 

The problems are PDOP, signal blockage, and multipath. GPS navi- 
gation requires line of sight reception of signals from as many different 
directions as possible. Pseudolites placed on the ceiling and walls of a large 
open space can meet that requirement. A room with a low ceiling, however, 
presents problems. Good VDOP requires at least one pseudolite to be above 
(or perhaps below) the user, and it is difficult to achieve this for all locations 
in the room without scattering a very large number of pseudolites across 
the ceiling. One might neglect VDOP by simply assuming that a receiver 
familiar with the room was at a known height from the floor. With that as- 
sumption, the horizontal position could be calculated (with good HDOP) 
using signals from pseudolites placed in the corners of the room. Unless 
the room is empty, however, the line of sight signals from those pseudolites 
will likely be blocked by other objects in the room. The receiver may track 
pseudolite signals reflected around obstacles and compute a position, which 
is incorrect due to multipath error, or it may fail to compute a position at 
all due to an insufficient number of signals. Both cases are problematic for 
reliable navigation. 

In a long, narrow room such as a hallway or tunnel, these problems 
are even worse. Unless a very large number of pseudolites are distributed 
along the ceiling and walls of the tunnel, a receiver will be unable to calculate 
either its height or its lateral position (across the width of the tunnel) with 
good DOP. The position along the length of the tunnel can be calculated, 
but only if the tunnel is sufficiently empty that the pseudolite signals are not 
obstructed. For these reasons, a conceptual “indoor GPS” system using pseu- 
dolites is generally impractical. 

Despite these difficulties, it is possible to create a pseudolite system, 
which can give a GPS receiver a general sense of its location indoors. The 
idea is to use the pseudolite as the source of a data signal rather than a naviga- 
tion signal [9]. Each indoor pseudolite can broadcast a preprogrammed signal 
describing its own location. A properly programmed GPS receiver that hears 
one of these pseudolites can report the pseudolite’s location as its own. Such 
a system would not allow precise navigation, but it could provide location 
information sufficient for many uses, such as directing emergency workers to 
a specific incident. The placement of such data-signal pseudolites could be 
adjusted to be as sparse or dense as the particular application requires. 

Unlike pseudolites for navigation, such data-transmitter pseudolites do 
not require precise clocks or synchronization systems. They can, therefore, 


Pseudolites for Vehicle Navigation 221 


be made quite small and inexpensive. The Japanese company GNSS Tech- 
nologies is working to commercialize this technology, which they call Indoor 
Messaging System (IMES) [10]. 


7.2 Pseudolite Systems 


At least three pseudolite systems were, are, or soon will be commercially avail- 
able. These are the IntegriNautics IN400/IN2400 system, the Locata Lo- 
cataLite system, and the Novariant (formerly IntegriNautics) Terralite XPS 
system. These three systems will be discussed in this section. 

Other pseudolites have been sold for research purposes, but did not find 
uses beyond research. Examples include the IntegriNautics IN200, IN300, 
and IN500, which were briefly available during the 1990s. Some researchers 
have constructed their own pseudolites, including Chris Bartone’s group at 
Ohio University in the United States and Changdon Kee’s group at Seoul 
National University in Korea. 

The original pseudolites, used in the 1970s in the original tests of the 
GPS concept, were built for the U.S. Air Force by Stanford Telecom. These 
were large complex devices, containing integral atomic clocks. They deserve a 
mention as the first pseudolites ever built, but they are long obsolete. 


7.2.1 IntegriNautics IN400 


The first pseudolite system actually used for ground vehicle control was 
the IntegriNautics (now Novariant) IN400 system. One such system was 
installed in an open-pit copper mine near Morenci, Arizona, operated by 
Phelps-Dodge (now part of Freeport-McMoran). The system consisted of 
several IN400 pseudolite transmitters, one IN2400 fixed reference receiver, 
and several IN2400 rover receivers. Figure 7.3 shows an overview of the en- 
tire system. Figure 7.4 shows one IN400 installed in the mine in 2001. 

The IN400 pseudolite transmitted a signal on the same L1 frequency 
used. by the GPS satellite system. The pseudolite signal itself closely resem- 
bled a GPS satellite signal, except that there was no attempt to synchronize 
the pseudolite’s internal timebase to any other source of timing. The pseu- 
dolite’s internal timebase was constrained by design so that the transmitted 
signal frequency was within about 2 parts per million of the nominal GPS 
frequency, which is within the expected Doppler frequency range of GPS 
satellite signals. 
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Figure 7.3 IntegriNautics IN400 pseudolite system (Novariant, Inc.). 


To mitigate the near-far problem, each IN400 pseudolite transmitted 
its signal in short pulses. The timing of the transmitted pulses was controlled 
so that the pulses from different pseudolites did not overlap. To achieve this, 
each IN400 pseudolite synchronized its pulse generator to an accurate pulse- 
per-second generated by a GPS receiver contained within the pseudolite. The 
microsecond-level synchronization required for the pulse pattern was far eas- 
ier to achieve than the nanosecond-level synchronization which would have 
been required to use the transmitted signal directly for precise positioning. 

Because the signals from the pseudolites were not accurate enough to 
use directly for positioning, the overall system operated differentially. A cen- 
trally located base station received all available signals from the pseudolites 
and the visible GPS satellites. At particular instants determined by GPS sys- 
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Figure 7.4 IntegriNautics IN400 Pseudolite in open-pit copper mine, Morenci, Arizona 
(Novariant, Inc.). 


tem time, the base station simultaneously measured the timing of each signal 
it saw, and then broadcast this set of timing measurements over a secondary 
data link. At the same instant, the mobile (rover) receivers measured the tim- 
ing of the signals they could hear, and then compared those measurements 
to the base station’s measurements that they received over the datalink. The 
rover receivers used the results of these comparisons to compute their posi- 
tions relative to the well-known location of the base station. 


7.2.2. Novariant Terralite XPS System 


As GPS systems have become more widespread, it has become more difficult 
to obtain permission to install pseudolites like the IN400, which transmit 
on the same frequencies as the GPS satellites. It is now essentially impossible 
to obtain a government license for a ground-based transmitter on any GPS 
frequency. This limitation, coupled with advances in technology, inspired 
Novariant to invent the Terralite XPS system [4]. 

Like the earlier IN400 system, the Terralite XPS system consists 
of a fixed ground constellation and any number of MX100 mobile rover 
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receivers. The ground constellation consists of several (often six) TX100 
Terralite transmitters (Figure 7.5) and one fixed IX100 Reference Station. 
The most obvious difference is that the Terralite XPS transmitters emit their 
signals in a frequency range known as the X-band, between 9.5 and 10 GHz. 
This band of frequencies is reserved under international treaties for radiolo- 
cation. This term has historically been used to mean “radar,” but pseudolite 
systems are also radiolocation systems according to international frequency 
regulations. It is thus comparatively easy to obtain licenses to install Terralite 
XPS systems anywhere in the world. 

The change to X-band also allows the Terralite XPS system to be much 
more precise than its pseudolite predecessors. The code chip rate in this band 
can be as high as 250 MBps, versus only 1 to 10 MBps in the GPS bands. The 
receiver’s ability to measure position changes is increased by a correspond- 
ing amount. The carrier frequency is about 6 times higher than the GPS L1 
frequency, so systems that use carrier phase measurements to determine posi- 
tion changes are more accurate as well. The major drawback to this frequency 
change is that radio electronics have been more expensive in the new band. 


_ 


Figure 7.5 Novariant TX100 Terralite XPS transmitter (Novariant, Inc.). 
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However, direct broadcast satellite television systems using similar frequen- 
cies are now in mass production, which has made X-band radio parts newly 


affordable. 


7.2.3 Locata LocataLites 


Locata Corporation, based in Canberra, Australia, has designed a consider- 
ably different navigation system based on pseudolite-style transceivers, which 
they call LocataLites. The Locata system has evolved through at least three 
generations of LocataLite designs and, as of 2009, was not yet available for 
general sale outside the research community. Many details of the Locata sys- 
tem remain proprietary, but it is possible to describe some features of the 
system from published papers [11]. 

LocataLites do not use GNSS satellite signals, except that the positions 
of some (not all) LocataLites must be determined at installation using a GNSS 
survey. Each LocataLite transmits navigation signals and receives signals from 
other LocataLites. This two-way communication allows them to synchronize 
their internal clocks with the system as a whole, and allows LocataLites that 
were not initially surveyed to determine their positions within the system. 

The signal format transmitted by LocataLites is not publicly docu- 
mented, but the signals are transmitted in the 2.4-GHz band. This is an “un- 
licensed” radio band worldwide, meaning that no special license is required 
to transmit radio signals in this band, provided that the transmitting equip- 
ment itself meets certain requirements. LocataLites meet these requirements, 
as do many other modern devices, including microwave ovens, Bluetooth 
headsets, and Wi-Fi wireless internet connections. The potential for interfer- 
ence among these devices is obvious, and in fact Locata has redesigned their 
signal format at least once to minimize interference from Wi-Fi connections 
[12]. The transmitted signals carry both timing information and data, just as 
GNSS satellite signals do. 

Destructive interference between direct and reflected (multipath) sig- 
nals can cause the received signal strength at a particular location to be low 
or even unusable. This is known in the literature as “signal fading.” Because 
the LocataLites do not move, the locations of severe fading of their signals 
also will not move. To combat fading, the current generation of LocataLites 
transmits signals on at least two frequencies within the 2.4-GHz band, and 
each LocataLite transmits through two antennas separated by about 2 meters. 
This improves the probability that at least one signal from one antenna will 
be usable at every point within the coverage volume of each LocataLite. 
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At system startup, none of the LocataLites know their clock offset or 
frequency errors, and many of them may not know their locations. As they 
take turns transmitting and listening to each other, the LocataLites synchro- 
nize their clocks and determine their relative distances from one another. 
Eventually the system converges, so that all the LocataLite clocks are synchro- 
nized and all the LocataLites have determined their locations relative to the 
few surveyed LocataLites. At this point, the system becomes operational and 
can be used for navigation. 

Each Locata rover contains a receiver for GNSS satellite signals and 
another receiver for LocataLite signals. The receivers are tied to a common 
time base, so that GNSS and LocataLite signals can be used together in any 
combination to determine the rover’s position. 
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Appendix 
Estimation Methods 
William Travis 


A.1 Introduction 


A brief overview of the estimation methods used in this book is presented in 
this appendix. A description of the underlying system model assumed for all 
derivations is provided, followed by techniques to discretize continuous sys- 
tems. The least squares method is derived first and used as the foundation for all 
other derivations. Weighted and recursive least squares derivations show how 
to use statistical information to determine the minimum variance estimate. The 
derivation continues by presenting the Kalman filter, which is able to incorpo- 
rate dynamic information about a system to produce an optimal state estimate. 
Finally, nonlinearities are handled in an extended Kalman filter. 

The reader is encouraged to study these topics in greater detail if a fun- 
damental understanding of estimation is desired. Excellent works dedicated 
to estimation methods are given in [1—5]. 


A.2_ System Model 


The derivations in this appendix operate on the assumption that a continu- 
ous system can be represented with a dynamics and measurement model in 
the following forms: 
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x(t) =f (x(t), u(t),t) +L()a(t) (A.1) 


2(t) =h(x(t),t) +v(t) (A.2) 


where the rate of change of x is a function of the state vector x and input 
vector uw at time ¢, plus an additive zero mean, white noise vector @, and the 
measurement vector is some function of x at time ¢, plus an additive zero 
mean, white noise vector v. The matrix L is the process noise gain matrix that 
relates the process noise to the state vector. 

The covariance matrices of @ and v are defined next. Note the assump- 
tion of no correlation between the process and measurement noise. 


E{aa' } =Q (A.3) 
E{vv' } =R (A.4) 
E{a@v! } =0 (A.5) 


In practice, one may encounter situations where process and measure- 
ment noises are correlated. Estimation algorithms can be derived to account 
for such a situation if necessary. 

The nonlinear expressions in (A.1) and (A.2) can be linearized about 
some operating region and written in a continuous state space format. 


x(t) =F(t)x(t) +G(t)u(t) +L) a(t) (A.6) 
2(t) =H (t)x(t) +v(2) (A.7) 
The matrices /, G, and H are defined as follows: 


F(t) x(2) |x(e).se) (A.8) 
Ge) ee oo (A.9) 
H(t) YEO (A.10) 


I(t) 
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A discrete state space representation is shown here. 
xp =F x,-, +Gy +, (A.11) 


Zp =Hpxp (A.12) 


The continuous process gain matrix, L, is absorbed into the discrete 
process noise, @, during the discretization process. If a direct discrete time 
model is defined, a discrete process gain matrix, j , can be defined. 


Xp =F Xb-1 +Gu, +j @, (A.13) 


A.3  Discretization 


Navigation system dynamics are continuous time processes, but implementa- 
tion usually occurs in a digital system. Discretization of the continuous time 
model can make the implementation process more convenient. The discreti- 
zation process occurs under a zero-order hold assumption, that is, the input 
remains constant between discrete time intervals, 


u(t) =u(tp) =ugp te Et <tpay (A.14) 


where the time interval is defined as follows: 


Dt =tp- - tp (A.15) 


The state transition matrix, F , maps the state vector from some time, 
t, to a future time, 7, during which time the matrix F is assumed to be 
constant. 


F(a) =e"? (A.16) 


x(T) =F (Gt)x(z) (A.17) 


The time intervals can be reduced if variation in F invalidates the as- 
sumption stated above. Multiple state transition matrices can be multiplied 
together to produce the matrix capable of mapping the state vector over the 
desired time interval. 


F (7,2) =F (7,7 QF (ct) (A.18) 
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A state transition matrix can be produced that maps the state vector 
over discrete time intervals by incorporating (A.15) into (A.16). 


F (¢estp-1) =F 4 eC 1) myPO (A.19) 
Using the matrix defined above, a state vector x,., can be mapped to x, 
xp =F pxp-1 (A.20) 


The other matrices can be discretized through integration. The discrete 
input matrix is calculated as follows: 


G =O F (tp,0)G dt (A.21) 
hl 


The discrete process noise and process noise covariance are similarly 


defined. 


Op =O, F (t,,T1)Lo@dt (A.22) 
k- 1 
Qz =9 F (tp,0)LQU! F (t,,1)! dt (A.23) 
b- 1 


Calculation of the discrete state space matrices using the formal defini- 
tions can be tedious, especially for complex systems. A method presented in 
[6] determines the state transition matrix and discrete process covariance ma- 
trix for time invariant or slowly time varying systems with a constant update 
interval. 


ef LOU 
@ 0 x £ 
Be F'Qt 
B=e4™ =6 . U (A.25) 
@0 aa : 


If the sample rate is sufficiently small, the state matrices can be easily 
approximated. 


F,»/+FDt (A.26) 
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G, » GDr (A.27) 


Qy » LQL' Dr (A.28) 


A4 Least Squares 


The goal of least squares is to determine an unbiased, minimum variance es- 
timate of the state vector x using available measurements. The estimated state 
vector is denoted by & The error between the estimated state and true state, 
or state residual, can be defined as: 


Ey =x- & (A.29) 
Ideally, this is a zero mean vector with some covariance given by P. 


Eleé,] =0 (A.30) 


Fle,e2 ] =P (A.31) 


A quadratic cost function is used to define the optimal state estimate. 
The cost function incorporates the square of the state residual. 


I(x) =e oe (A.32) 


Taking the square of the state residual guarantees a local minimum. 
The cost function, as defined, will fluctuate only with the magnitude of 
the state residual and therefore is independent of the sign of the state re- 
sidual. This effect is depicted in Figure A.1. One could define the optimal 
state estimate by defining the cost function as the absolute value of the 
state residual. However, a discontinuity in the derivative of the absolute 
value operation at zero makes the derivation of the optimal state estimate 
difficult. 

This state residual is often impractical to calculate, especially in real- 
time systems, as the true state is rarely known precisely. However, a measure- 
ment prediction, 2, can be formed given the state estimate. 


2 =Hx (A.33) 
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0.6 0.2 
(x) =e, 
un 0.15 
0.2 
0.1 
0 
a 0.05 
—0.4 0 
-2 0 2 -2 


(a) (b) (c) 


Figure A.1 (a) A true and estimated state. (b) A cost function is defined as the state re- 
sidual. Notice how the error changes signs, becoming more negative as the 
independent variable increases. (c) A cost function is defined as the square of 
the state residual, where only the magnitude of the error fluctuates with the 
independent variable. A minimum is created, occurring at (0, 0). 


The error between the true and predicted measurements can be deter- 
mined. Note that this error, known as the measurement residual, contains 
not only errors in the estimated state, but also errors in the measurement. 


€, =z- 2 (A.34) 


The cost function can be redefined to use the readily available informa- 
tion in the measurement residual. 


I(e)=,€2 e. =e Hi)! (z- HE) 
i (A.35) 


Az! z- z’ He- 2’ Hz +27 HT! H%) 


The optimal estimate of the state vector will be one that minimizes the 
magnitude of the error and therefore will minimize the cost function. A nec- 
essary condition for the existence of the minimum is the gradient of the cost 
function, or Jacobian, must equal zero when evaluated at the minimum. 


NsJ(z) =H! A&- A" z =0 (A.36) 
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The normal equation is derived from the above expression: 
H’ He =H" z (A.37) 
The second derivative of the cost function is known as the Hessian. 
NZ /(z) =H’ H (A.38) 


The resulting matrix will be an 2 © » symmetric matrix. A sufficient 
condition for the existence of a minimum is that the Hessian must be posi- 
tive definite. Positive definiteness is obtained when the rank of H is 7, which 
means a minimum of 7 linearly independent observation equations exist. If 
the sufficient condition is satisfied, the least squares estimate of the state vec- 
tor is defined as follows: 


&, =(H, Hy) Ay ze (A.39) 


Example A.1 


A gyroscope is placed on a rate table for calibration. The rate table is ex- 
tremely accurate, but the gyroscope output is corrupted with a constant scale 
factor and constant bias. Use least squares to determine the scale factor and 
bias. 

A model of the gyroscope output must be constructed first. 


Zz =ay tb +v (A.40) 


A scale factor, a, scales the true rotation rate given by the rate table, y, 
which is also biased by some amount, 6, and corrupted by white noise, v. 
With the understanding that multiple measurements are recorded, the model 
can be rewritten in matrix format. 


eay, +6 +; U én lu év; U 
ue uz. @ U 
Say thu. Sy, Ye So. ¥ 
=o" PURO? UR +E U=ae to (AAI) 
é U6 i Uumy é: U 
e ue Uu e u 
Gm FO +VmG Gm WA  Mmd 


Note that linear independence among the observations is achieved not 
by the number of measurements, but by the variation in the actual rotational 
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rates observed. Measuring a single rotational rate does not provide enough 
information to estimate both the scale factor and bias terms, as their effects 
are indistinguishable. In the case of m measurements of the same rate, H can 
be written as follows: 


en lu 
e Uu 
av] ile 
H=2' U (A.42) 
ei y 
€ UW 
el 1G, el 


where resulting Hessian is symmetric, but not positive definite as the rank is 
equal to one. 


So aan 
HH=e OM (A.43) 
ey mC 


Operating the table at two or more distinct rates increases the rank of 
H™H to two. The following Hessian assumes m measurements with distinct 
rotational rates, and its rank is two. 


Pege 2% yZ tty oy ty te tym 
an ty2 t+ tym m 


(A.44) 


COoCwce 


If the sufficient condition is met, the least squares estimate of the scale 
factor and bias can be calculated with (A.39), where = [4 47. 


A.5 Weighted Least Squares 


More often than not, available measurements are not equal in terms of qual- 
ity. The regular least squares method does not account for statistical differ- 
ences among the measurements. A simple addition of a weighting matrix 
to the cost function adds the capability to weight some measurements over 
others to improve the overall accuracy of the state estimate. Weighting is pro- 
portional to the anticipated quality of the measurement; the weight should 
approach infinity as the quality increases, and it should approach zero as the 
quality decreases. 


Appendix 237 


Given a series of measurements at time &, a weight is assigned to each. 
zy =A x, +0, ® Wy 
22 =Hoxp +02 ® Ww2 

(A.45) 


Bm =H xp tVy ® wy 


The weights are placed into a diagonal weighting matrix, W, where each 
element is in the row corresponding to its measurement. 


ay sesd 0 L 

—¢e : L 
Wh ‘ (A.46) 

@0 ee Wn 


The cost function in (A.35) is rewritten using the weighting matrix. 
LT 1 a\T , 
T(z) ae We, mae - Hx) W(z- Hx) 
(A.47) 


1 
=5 (2! Ws - z! WHR - & H' We +8" H! WHR) 


The Jacobian, normal equations, and Hessian are updated accordingly. 


Ne/(z) =H’ WHR - H' Wz =0 (A.48) 
H! WHR =H! Wz (A.49) 
NZ /(z) =H’ WH (A.50) 


The weighted least squares solution is expressed as follows: 
A —zzl -lyp7T 
xp =(H, Wee) A, Wree (A.51) 


If statistical information of the measurements exists, the inverse of 
the covariance of the measurements can be used as a weighting matrix. The 
weighted least squares estimator produces the best linear unbiased minimum 


238 GNSS for Vehicle Control 


variance estimate, in a statistical sense, when using the inverse of the measure- 
ment covariance as a weighting factor. 

The measurement covariance, R, is defined as the expectation of the 
square of the measurement noise, assuming all measurement noise is zero 
mean. 


R =cov(v) =E[vv" ] (A.52) 


The weighting matrix can be defined as the inverse of the measurement 
covariance matrix. 


W =R! (A.53) 


The linear, unbiased, minimum variance estimator is typically expressed 
as follows: 


fas Vt cor ne 
Rp =(A7 R;'A1) HR, ze (A.54) 


Often, it is useful to express the estimator in terms of an estimate of the 
covariance of the estimation error, P, which is defined in (A.31). 


- -1 
P, =(A7 R; Ag) 
(A.55) 
Sy =PH] R,'z4 


The term (H’R'!H)' maps the known statistical information of the 
measurement vector to the state vector. Therefore, the matrix P should be a 
symmetric, positive definite matrix that contains statistical information of the 
estimated state vector (i.e., P = E[e€, €7]). Note that the expression in (A.55) 
can be used in lieu of (A.51), but caution is advised when analyzing because 
P can be arbitrary due to the fact that elements in W might not be accurately 
selected. 


Example A.2 


A localization system consists of four radio beacons at known locations and 
a receiver. The receiver uses the signal broadcast from a beacon to determine 
a range measurement to that beacon. Therefore, the receiver in the described 
system shown in Figure A.2 will provide four range measurements to four 
known locations, measured in units of meters. The beacons are located at the 
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Figure A.2 A user wants to determine his position in a local, two-dimensional coordi- 
nate system. The black dots are the location of the radio beacons, and the dot 
marked with an X is the user's actual location. The user has a receiver that 
measures the range from it to the radio beacons. 


following points in a local, two-dimensional frame: (1,300, 1,000); (800, 
1,100); ( 1,000, 1,200); and ( 1,100, 1,100). The range measurements are 
corrupted by white noise, but the amplitude of the noise is dependent upon 
the distance between the beacon and receiver. The noise standard deviation 
was experimentally determined as 6, = Ur, where 7 is the true range and [Ul is 
a scaling factor. The measurements have no correlation with one another. A 
user wishes to determine his static position within a local, two-dimensional 
reference frame. Use the minimum variance estimator to determine his op- 
timal position. 

A range measurement can be modeled as the norm of the difference in 
position between the radio beacon and the user location, where r/ is the range 
from the user to beacon, e, and 7, are the respective east and north compo- 
nents of the user position, and ¢ and 7’ are the respective east and north 
components of the beacon position. 


1 = Flemm)=V\ (ene? + (my n°? (A.56) 


An estimate of the user position can be used to calculate a range esti- 
mate, since the user position is unknown. 
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if =f (- A= - ) +f, - nb) (A.57) 


The measurement residual, €, = r — ?, which is the basis of the cost 
function, is formed using (A.56) and (A.57). However, these expressions are 
nonlinear at this point and therefore cannot be placed into a state space for- 
mat. Ideally, the measurement residual is zero. The following expression can 
be formed assuming a zero measurement residual. 


f (Custu) =f (bustin) (A.58) 


This equation can be linearized with a Taylor series expansion where 
the zero- and first-order terms are retained to develop an expression for po- 
sition error as a function of the range measurements and beacon positions. 


Expanding (A.58) leads to the following: 


pis 4 v4 QVCnrtiv)) 8g PW Cwrtiv)) 
Fleur) =f Curtin) Fg {é, burda phen én) tg V) barby alu My) 


(A.59) 


where f(e,, 7,) is the measured range; f(é,, 7,) is the position estimate calcu- 


Vesta) 4 WU Cvstn) 


"é u My 


lated with the right side of (A.57); 


are the compo- 


nents of the unit vector pointing from the receiver to radio beacon; and 
(e, — é,) and (7, — ,), which can be expressed as De and Dy, are the er- 
rors in the estimated state. Placing this information into (A.59) yields the 
following: 


rf =. ee y +A, - nb) +u,De +u,,Dn (A.60) 


where uw, and u,, are the unit vectors described above. 


b 
_ Ce - ey 
eee 2 2 
Ile - ra +(A, - n’) 
(A.61) 
_ i me 
Un = 


2 2 
(a, - e) +(i, - n’) 
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The only remaining unknown terms in (A.60), with initial approxima- 
tions of é, and 7,, are the errors in the estimated state. The equation can be 
rewritten and placed into a zg = Hx form, where m = 4 for this example. 


be N= OY Ha PP Stee De (A.62) 
é 3 2t 
Bri - (a, - ef) +(4, nf | U 
e . L 
e L 
x h go ats 5° 
Bim 7 le - e!,) +i, nb) t 
Cute} Un) U 
H= : (A.64) 


(A.65) 


An attempt to estimate the position can be made at this point. How- 
ever, the statistical quality information of the measurements would not be 
utilized. The covariance matrix is formed with the estimated range since the 
noise is dependent upon distance. 


y b\2 t 
A ust) 0 ¢ 

R= ( (A.66) 
é ab \2U 


After each iteration, the position estimate should be updated by adding 
the error term to the original linearization point. 


é, =e, +De 
(A.67) 
ty =n, +Dn 
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Figure A.3 The state residual for east position is plotted. The black dashed lines represent 
the estimated standard deviation of the residual. 


Figures A.3 and A.4 were generated by simulating this example with 
an initial position estimate of (0, 0) and uw = 0.001. Figure A.3 displays the 
state residual for the east position and the estimated standard deviation. The 
estimated standard deviation was determined by taking the square root P),. 
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Figure A.4_ A plot of estimated north and east position centered around the true position, 
which is denoted by X. 
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Figure A.4 shows the estimated position compared to the true position. The 
estimated points are bounded by a circle around the true position. This ex- 
ample presented an overdetermined solution with near optimal geometry, so 
different beacon locations and/or a different quantity of beacons would alter 
the shape of the bounding region. 


A6 Recursive Weighted Least Squares 


The previously derived estimation formulas given in (A.39) and (A.55) are 
batch processing algorithms in the sense that all available data at some time 
k is used to compute an estimate. Estimates can be determined with a single 
batch of data where all data prior to and including time &. Sequential batch 
processing would use only information available at time &. One possible hin- 
drance to sequential batch processing is the estimated state vector does not 
rely upon previous measurements; its value is determined solely from the ob- 
servations at &. Real-time applications present the challenge of noncoincident 
information availability and data storage limitations, so an ideal estimation 
algorithm would process measurements sequentially to update the state vector. 
The recursive least squares algorithm processes available measurements at each 
instance in time to computes an estimator gain. This gain is multiplied by the 
measurement residual to determine the adjustment to be applied to the state 
vector. The result is equivalent to batch processing all past and current data. 
Given two measurement vectors, sequential in time from & 1 and &: 


Ze 1 = Hp 1x+ Vp 1 


(A.68) 
Zp = Hpxt+v, 
where the least squares estimate of the state at time & is given by 
1 
Se 1=(AER, Hes) AER, Ze (A.69) 


The sequential measurements can be grouped to determine the state 
estimate at time & using measurements from both epochs. 


z=Hx+vd (A.70) 
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The estimate %;,; is computed using the grouped measurements. 


-1 
ty =(A7R A)! TR's (A.72) 
This equation can be expanded as follows: 


- - a - - 
fp =(H7 Ry. Hee +H R; Hy) (at. Ry zh +HzR;, ‘ze) 
(A.73) 


Equation (A.55) presented 
—(rT p-1 es 
Pe. =(H7 Re) (A.74) 
so P, can be similarly defined. 
(rT p-1 ieee a coe Pipi Vt 
Using the matrix inversion lemma, P, can be redefined. 
7 i ig 
P, =P,-1 - Pp, (Hi 2. 1H, +R) HyP- (A.76) 


Equation (A.76) can be substituted into (A.73) to determine a recursive 
expression for the state estimate update. 
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I =(47,..H7 +R, \" (Hepa? +R) (A.78) 
#1] 
Kp =Xp-1 +P,H |, (42. \Hy +R) * (ze - Hy&e-1) (A.79) 


The estimator gain, K, can be pulled from the recursive equation to 
update x. 


ae Zz ‘a et 
Ky Hh, (a7. 1H, +R] (A.80) 


The estimator gain is a function of the prior estimated state covariance 
and the current measurement and measurement covariance matrices. The 
gain is multiplied by the residual between the current measurement and an 
estimate of the current measurement based off of the previous state estimate. 
The resulting vector is an adjustment added to the previous state estimate to 
determine the current state estimate. The recursive least squares algorithm 
can be summarized as follows: 


=1 
Ky =Ph.1Hy, (142,. 1H, +R, ) 
Xp =hp-1 +Ke (ze - AeXe-1) (A.81) 


P, =(7;1 +H} RH y' 


Notice that the estimated state error covariance is a function of the 
measurement matrix and measurement covariance matrix, and the gain is re- 
liant upon the estimated state covariance matrix, but neither have any depen- 
dence on the actual value of the estimated state. Because of this relationship, 
or lack thereof, an accurate state estimate does not guarantee a correspond- 
ingly accurate covariance, and an acceptable covariance value does not in- 
dicate a correct estimate. Fortunately (or unfortunately), the fault of these 
occurrences can usually be assigned to the user, and not the algorithm, in the 
form of modeling error. 

The fact that modeling error would corrupt an estimator’s output is 
quite obvious, but it isa common occurrence nonetheless. The model may 
contain an invalid assumption of the signal. For instance, (A.56) contains 
the assumption that the beacons and receiver operate on a common, perfect 
clock. In reality this can never be the case, and therefore any clock biases 
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and delays will cause the output to deviate from its true value. This error 
would manifest itself in the state estimate, and the residuals would become 
biased. 

It is important for the user to realize that recursive least squares should 
not be used to estimate dynamic states. This would be a form of a model- 
ing error, as the user improperly assumes estimated signals will be static. 
The gain matrix approaches zero as time increases, which diminishes the 
influence of the measurement residual on the state update. Any changes to 
the state would effectively be de-weighted and heavily filtered. The follow- 
ing section discusses the Kalman filter, which uses knowledge of a system’s 
response as a function of time to propagate the state and state covariance 
forward in time. 


A.7 Kalman Filter 


If a system’s model and initial conditions are known exactly, the states can 
be propagated forward in time, possibly based on some external input, with 
an infinite degree of certainty. The need for an estimator would be nonex- 
istent. Consider the case where an airplane departs Atlanta, Georgia, trav- 
eling towards Dallas, Texas. The flight distance is known exactly, and the 
fuel consumption model is perfect. To save money, the airline puts the exact 
amount of fuel necessary to get the plane from Atlanta to Dallas, which was 
determined by the model. 

Most people would not board that plane with this knowledge. What 
if the plane’s route is diverted around a storm, a headwind is encountered, 
or the cargo is unusually heavy? Reality dictates uncertainty in the model 
and initial states. Modeling inaccuracies will usually exist due to assump- 
tions made when deriving the model, effects experienced during operation 
that were unaccounted for when deriving the original model, or unknown 
system components that affect the behavior. Variation can exist among ini- 
tial states even in highly controlled environments. Propagation of a system 
model will quickly produce inaccurate estimates, but external measure- 
ments of the states can be used to correct any drift in the estimates that 
may occur. 

The previous section discussed the use of stochastic information to 
combine weighted measurements and produce a minimum variance esti- 
mate. The state vector could not be defined exactly, but an estimate of the 
state covariance provided a quality indicator of the state estimate accuracy. 
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The Kalman filter incorporates dynamic and stochastic information with 
measurements of the states to produce an optimal, nonbiased, minimum 
variance estimate. Estimates and their covariance are propagated from time 
k 1 to k using a mathematical model of the system’s dynamic and any 
known input. Measurements of the states are recorded, and the estima- 
tor gain, K, is optimally determined using knowledge of the measurement 
quality and information about process noise entering the system. Once the 
gain is determined, the states and their covariance are updated with the 
measurements. 
Propagating the state forward in time uses the state transition matrix, 
4 v and a deterministic input, u, if one exists. The following equation 
propagates the state estimates. 


Xp= Xeyt upy (A.82) 


Note that the subscripts on the system matrices have been dropped for 
simplicity. 

Covariance propagation starts by defining the covariance at successive 
instances in time. 


Pea=P x58), (A.83) 
Be G6 (A.84) 


Noting that the state transition matrix can also be used to propagate 
the error, 


Ex, = Ex, + We 1 (A.85) 


Equation (A.82) can be rewritten as follows: 
Bh=E ( €m,+ Obi) Eq, + Ob 1) (A.86) 


Expanding the expression inside the expected value operator and then 
evaluating the expected value of the terms yields the discrete expression to 
propagate the estimated state error covariance. 
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Next, the optimal Kalman gain is calculated. It is a function of the 
process and measurement covariance matrices, and therefore the optimality is 
conditioned upon the accuracy of the contents of these matrices. 


=] 
K, =Pj Hy, (a7 cea +R] (A.88) 


The measurement update is the final step of the iterative process. The 
state is updated by adding the previous estimate to the gain multiplied by the 
measurement residual. 


aif =2, +K,(ze- Here) (A.89) 
P+ =(1- KH Pi (A.90) 


The measurement residual, z, — H,%;, is the error between the actual 
measurements and predicted measurements. Therefore, it should consist of 
zero mean, white noise vectors if the filter is functioning correctly. If any bias 
exists in the residual, the filter is improperly tuned or the model is inade- 
quate. The residual can be monitored online to adapt the covariance matrices 
or reject potentially bad or corrupt measurements. 

Note that the estimate of error covariance is independent of the state 
vector. This offers the designer the opportunity to predict the estimator 
performance through simulation. Various models, different tunings, or a 
mixture of sensors can be simulated to determine the best combination 
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that achieves the desired estimate accuracy. For some systems, the gain and 
covariance can be computed offline to reduce the computational load at 
runtime. 

The Kalman filter algorithm is summarized here. 


Time Update 
Kp =F pXp-1 +Gpug 
Py =F pPpiF i +i cQui, 


Gain Calculation 


HA A91 
Ky, =PHT (H,P.HT +R) ee 


Measurement Update 
Kp =p +Kp (Ze - HeXe) 


P, =(p;' +H! R'H,) 


The dynamics model given by F and Gis used to propagate the state 
from one instance in time to another. The model also provides a method of 
predicting the states’ motion, so the Kalman filter can be used to estimate 
dynamic states. Also, notice the addition of the process noise covariance to 
the propagated state error covariance in the time update. This term prevents 
the gain from approaching a steady state value of zero, so the effectiveness 
of the measurement update is retained as time approaches infinity. 


A. Extended Kalman Filter 


All estimators to this point have been derived with the assumption of a linear 
system and measurements. In practice, most systems are inherently nonlin- 
ear, and therefore the estimators must be altered to accommodate the nonlin- 
earities. The following discussion will involve the Kalman filter; application 
of linearization techniques to the other least squares estimators should be 
obvious afterwards. Specifically, the focus will be on the extended Kalman 
filter. Other methods are available for handling nonlinearities in systems and 
measurements such as the unscented filter and particle filter, but the extended 
Kalman filter provides acceptable performance with the least computational 


burden for ground vehicle dynamics with measurement rates from 1 Hz to 
500 Hz. 
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The extended Kalman filter is capable of handling nonlinearities in the 
system. Linearization of the nonlinear dynamics and measurement models 
provides the necessary information to compute the Kalman gain and update 
the estimates. However, there is a sacrifice. The minimum variance guarantee 
does not exist for the extended Kalman filter as the minimization process can 
focus on a local, rather than global, minimum variance. This is more likely to 
occur when initial condition errors are large, which corrupts the information 
contained in the linearized state matrices. 

The extended Kalman filter operates in principle like the linear Kal- 
man filter. The differences are that the nonlinear expressions must be lin- 
earized each iteration with the current estimate and the time propagation 
and residual calculation are performed with the nonlinear equations. The 
components of an extended Kalman filter can be mixed with those in the 
linear Kalman filter depending on where the nonlinear relationships exist. 
The nonlinear expression must be integrated to propagate the states of a sys- 
tem with a nonlinear dynamics model and linear measurement model, but 
(A.10) can be used to compute the measurement prediction. Alternatively, 
(A.8) and (A.9) can be used to propagate the estimates of a system with 
a linear dynamics model, but (A.10) cannot be used to calculate the state 
measurement update. 

The nonlinear state vector propagation requires numerical integration, 
so an appropriate routine, such as Euler, trapezoidal, or Runge-Kutta integra- 
tion, must be selected by the system designer. The propagation is the evalua- 
tion of the differential equation describing the system’s dynamics. 


Se =8E tO f(&(o),ule),t)dt (A.92) 
1 
The covariance is propagated accordingly. 


Pe =Pr, +0, F(t)P(t) +P(t)F! (t) +L(t)Q(Q)L (a)dt (A.93) 


The matrix F is obtained by evaluating (A.8) with the most recent esti- 
mate, and Q is the continuous process noise covariance. 

The gain computation is unchanged from the linear version of the Kal- 
man filter. The measurement model is linearized with (A.10) at the most 
recent estimate, if necessary. 
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Ky =Pi Hi (He Pi Hi +R)! (A.94) 


The state update utilizes the nonlinear measurement model to form the 
predicted measurement vector (i.e., 2, = A(Xpt,)). 


ae =k, +K (ze - A(xe te) (A.95) 


The covariance update equation remains unaltered, but uses the linear- 
ized measurement matrix. 


PY =I - KpH,) Ph (A.96) 


The extended Kalman filter algorithm is listed here. 


Linearize System 


FQ) = VCO.) 
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Discretize System (A.97) 
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Time Update 
a sat +0. f(&(t) +u(t), t)dt 
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Gain Calculation 
Ky =PH, (ApP.H, +e) | 


Measurement Update 
Rf =e +Ke(ze - blob te)) 
PY =U - Kee) Pi 
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A.9 Initialization 


Initialization of a Kalman filter is a matter of providing an educated guess of 
the initial conditions with some associated uncertainty. 


ho U é 42 eee a 
GU & On POP xn 
fy =P URT =O: -_ (A.98) 
a : ee ge 
gf B05 a 


Inputs might range from being mostly arbitrary to very precise. The 
accuracy of the initial conditions can affect filter performance, especially 
when using an extended Kalman filter. In some applications, measurements 
are available before the Kalman filter is started. Measurements of the ap- 
propriate states can be used to initialize the state vector. This is often useful 
as the operator usually has good knowledge about the accuracy of the mea- 
surements, and therefore this accuracy can be used to initialize the covari- 
ance matrix. 
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See also GNSS ground vehicle control 


Lateral position, 198-202 


classical method, 200 
closed-loop block diagram, 200 
closed-loop poles, 202 

control, 200-2 

model, 198-200 

open-loop poles, 202 
state-space method, 201-2 


Least squares, 233-36 


estimate of state vector, 235 
example, 235-36 

goal of, 233 

recursive weighted, 243-46 
weighted, 236-43 


See also Estimation methods 


Lidar-based positioning 


advantage, 42 
defined, 39, 40 
echo width, 41-42 
illustrated, 41 
scanners, 39, 40-41 


See also Lane positioning 


Linear inertial instruments, 18—20 
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Linear quadratic regulator (LQR), 190 
Linear tire model, 161-64 
Lines, error calculation for, 194-96 
Local area differential GPS (LADGPS), 7 
Locata LocataLites, 225-26 
clock synchronization, 226 
defined, 225 
navigation signals, 225 
rovers, 226 
signal format, 225 
See also Pseudolites 
Longitudinal dynamics, 68-70 
Look-ahead distance, 192 
Loose coupling, 93-94 
benefits/drawbacks, 94 
defined, 93 
Low-u test 
Dugoff tire parameters, 169 
estimated tire curves, 159 
estimated tire parameters, 159 
nonlinear model-based estimator, 


167-69 


Magic Model, 120-21 
MATLAB simulations 
dynamic equation, 84 
two-state roll plane model, 87 
Measurement residual, 234, 240 
Measurement vector, 139 
Model-based Kalman filter, 160-71 
covariance analysis summary, 171 
covariance values, 171 
defined, 161 
estimator accuracies, 170—71 
linear tire model, 161-64 
nonlinear tire model, 164—70 
robustness, testing, 166 
sensor options, 162 
See also Kalman filter 
MTSAT Satellite-based Augmentation 
System (MSAS), 8 


Navigation filter, 48-49 
closely coupled, 52-59 
initialization, 48 

Navigation systems, 91-143 
closely coupled integration, 130-43 


GPS/INS integration architectures, 
93-95 
heading estimation, 107-11 
introduction to, 91-92 
Kalman filter, 92-93 
position, speed, and heading estimation, 
111-19 
sideslip and, 120-30 
speed estimation, 95-106 
Network differential GNSS navigation 
(NDGPS), 7 
Neutral steer, 71 
Newtonian dynamics, 155 
NMEA RMC message, 113, 116, 122, 123 
contents, 92 
course over ground measurement, 107 
latitude and longitude, 112 
longitudinal and lateral velocity, 123 
measurements, 111 
Noise 
IMU, 137 
inertial instrument error, 26 
process, 114, 137, 153 
sensor, 153 
Noise error, 26 
automotive grade, 27 
modeling, 26 
Nonlinear tire model, 164-70 
North East Down (NED) coordinate 
frame, 47-48, 54-55 
Notational style, 132 
Novariant Terralite XPS system, 223-25 
frequency change, 224-25 
ground constellation, 223-24 
inspiration, 223 
precision, 224 
transmitter illustration, 223 
See also Pseudolites 


Odometers 
output, 102 
quantization, 32, 33 
technology, 31-33 
wheel radius error, 33 
wheel slip, 32-33 
Open-it mining, as pseudolite application, 
216-17 
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Optical gyroscopes, 20 
Orthogonality errors, 30 
Oversteer, 71 


Position 
accuracy, 119 
ECEF, 133 
lateral, 198-202 
states, 139 
Position, speed, and heading estimation, 
111-19 
coordinate conversion, 112-13 
defined, 111 
See also Navigation systems 
Position DOP (PDOP), 6, 220 
Precise (RTK) GNSS navigation, 8-11 
PRN codes, 9 
Process gain matrix, 126, 138 
Process noise, 137 
covariance matrix, 114, 153, 157 
discrete, 163, 232 
Proportional-derivative (PD) controller, 
181, 200 
Proportional-integral (PI) controllers, 
180, 182 
Pseudolite applications, 215-21 
construction sites, 218 
indoor, 219-21 
open-it mining, 216-17 
urban navigation, 218-19 
Pseudolites, 14-18, 215-26 
basics, 14 


conflicts with GNSS frequencies, 17-18 


defined, 14 
differential, GNSS navigation, 15-16 
GNSS navigation, 14-15 
IntegriNautics IN400, 221-23 
Locata LocataLites, 225-26 
Novariant Terralite XPS system, 
223-25 

self-synchronization, 16 
stand-alone navigation, 16 
systems, 221-26 

Pseudorange, 131 
accuracy, 140, 141 
measurement geometry, 4 
measurements, 140, 141 


rate, 140, 141 


Quantization, odometer technology, 32, 33 
Quasi-Zenith Satellite System (QZSS), 
12-13 


Recursive least squares (RLS), 156 
algorithm, 245 
regression matrix, 157 

Recursive weighted least squares, 243—46 
estimated state error covariance, 245 
estimator gain, 245 
grouped measurements, 243-44 
measurement processing, 243 
See also Estimation methods 

Roll model, 79-80 
equation of motion, 80 
free body diagram (FBD), 79 
state space representation, 80 
testing, 86 
validation, 87 

Rotational rate vectors, 132 

Rotation matrices, 43—44 
defined, 43 
from ECEF coordinate frame, 53 
illustrated, 44 
three-dimensional rotations, 45—46 
two-dimensional rotations, 44-45 


SAE Vehicle Coordinate System, 61-63 
defined, 61 
illustrated, 62 
Scale factor, 235 
Self-synchronization, pseudolite, 16 
Sensor noise, 153 
Sideslip, 120-30 
angle, 146 
bicycle diagram, 122 
calculation, 146-47 
compensation, 122-30 
defined, 128 
direct estimates of, 163 
estimate comparison, 130 
estimation (asphalt experiment), 164 
estimation (two-antenna GPS), 167 
false estimation, 151 
generation of, 120-22 
high rate estimate, 129-30 
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Sideslip (continued) 
measurement with yaw rate gyro, 
126-28 
navigation and, 120-30 


with only yaw rate measurement, 161 
slalom section (asphalt experiment), 165 


Skew symmetric form, 133 
Slip angles, 122 


Space-based augmentation system (SBAS), 


8 
Speed controller, 179-81 
Speed estimation, 95-107 


accelerometer, GPS, and wheel speed, 


102-6 
accelerometer and GPS, 96-102 
accuracy, 102 
drift, 101 
error, 102 
variance, 101 
See also Navigation systems 
Speed measurement 
GPS, 97, 98, 106 
residual, 99 


Stand-alone pseudolite navigation, 16-17 


State covariance, 99 
State feedback, 145 
State feedback controller, 188—89 
State residual, 233, 242 
State-space model, 188-90 
State transition matrix, 135, 231-32 
State vector, 97 
Steer angle 
bias, 163 
controller, 181-82 
reference, 197 
Strap-down IMU, 131 
System model, 229-31 


Temperature errors, 30 
Three-dimensional map construction, 
54-56 
Time DOP (TDOP), 6 
Tire parameter 
from asphalt experiment, 158 
assumptions, 155 
identification, 154-60 
lateral forces, 155 


from low-u test, 159 
validation, 157 

Tires, 74-78 
basics, 74 
bicycle model comparison, 86 
circle of friction, 76 
contact patch, 74-76 
cornering stiffness, 120 
curves, 121 


Dugoff model, 78, 120-21, 155-56, 


166, 169 


Fiala model, 76-78 
functions, 74 
generic curve, 75 
lateral slip, 121 
linear model, 161-64 
models, 76-78 
nonlinear model, 164-71 
vertical forces, 75 
Tire slip, 74-76 
angle, 75 
longitudinal, 103 
steady-state, 67 
Tracking loop parameters, 140 
Trailer control, 208-10 


along line and arc, 211 
arc segments, 209 
error equations, 208, 209 


gains, 209 
line segments, 209 


parameters, 210 
point, 205 


vehicle control comparison, 210 
weighting values, 210 
Trailer model, 82-83, 204-6 


free body diagram, 82 

heading, 208 

hitch angle, 83, 205, 207 

hitch dynamics, 205 

kinematic model, 204 

lateral dynamics, 205 

zero lateral velocity (ZLV), 82-83, 203 


Two-dimensional map construction, 50-51 
Two-dimensional rotations, 44—45 
Two-wheeled vehicles, 81-82 


kinetic diagram, 81 
state space representation, 82 
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Understeer gradient 

defined, 70 

determination, 71 

neutral steer, 71 

oversteer, 71 

plot, 71 

understeer, 71 

See also Bicycle model 
Unmanned ground vehicles (UGV), 91 
Urban navigation, as pseudolite 

application, 218-19 


Vehicle dynamic control (VDC), 91 
Vehicle dynamic estimation, 145-72 
conclusions, 171-72 
experimental setup, 148-49 
introduction to, 145-46 
kinematic estimator, 149-51 
kinematic Kalman filter, 151-54 
model-based Kalman filter, 160-71 
sideslip calculation, 146-47 
test scenarios, 148-49 
tire parameter identification, 154-60 
vehicle estimation, 147 
Vehicle heading, direct estimates of, 163 
Vehicle modeling, 61-88 
bicycle model, 63-74 
roll model, 79-80 
SAE Vehicle Coordinates, 61-63 
tires, 74-78 
trailer model, 82-83 
two-wheeled vehicle, 81-82 
validation, 86-88 
Vehicle models, 175-79 
block diagram, 177 
center of gravity (CG), 176 
control design methods, 178 
derivation, 176 
diagram, 176 
parameters, 179 
requirement, 175 
steering system, 177 
throttle, brake, steering wheel, 177 
See also GNSS ground vehicle control 
Vehicle steering control, 181-85 
steer angle controller, 181-82 
trailer control comparison, 210 


yaw rate controller, 181, 182-85 
Velocity 
ECEF, 133 
GPS measurement, 151 
measurements, 139 
vehicle, 143 
Vertical DOP (VDOP), 6, 220 
Vibrational gyroscopes, 20 
Vision aided navigation systems, 39-59 
Vision measurements, 49-52 
aiding closely coupled navigation filter 
with, 52-59 
results, 52 
structure, 51 
two-dimensional map construction, 
50-51 
waypoint map position, 51-52 


Waypoint-based maps, 48-49 
Waypoint control, 185-92 
defined, 185 
heading control, 187-89 
heading error calculations, 186-87 
heading model, 185-86 
look-ahead distance, 192 
simulation results, 190-92 
See also GNSS ground vehicle control 
Waypoints 
condition failure, 186 
list of, 186 
map position, checking, 51-52, 58 
selection, 186 
simulation heading error, 191 
simulation path, 191 
testing, 186-87 
in two-dimensional map construction, 
50 
Weighted least squares, 236-43 
covariance matrix, 241 
example, 238-43 
measurement residual, 240 
recursive, 243-45 
solution, 237-38 
state residual, 242 
weighting as proportional to quality, 
236 
weight placement, 237 
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Weighted least squares (continued) 
See also Estimation methods 
Wheel radius error, 33 
Wheel slip, 32-33 
Wheel speed, 102-6 
accelerometer, yaw rate gyroscope, and 
GPS, 113-19 
illustrated, 105 
model, 104 
quantized nature of measurement, 105 
Wide Area Augmentation System (WAAS), 
8 
Wide area differential GNSS navigation 
(WADGPS), 8 


Yaw rate 
control, 181 


direct estimates of, 163 

gains, 183 

slalom section (asphalt experiment), 165 
Yaw rate controller, 182-85 

bicycle model Bode plot, 184 

closed-loop block diagram, 183 

development process, 182 

form, 182 

root locus design, 183 

third pole location, 184-85 
Yaw rate gyroscopes, 113-19 

bias, 118, 128 

error, 128 

measurement, 123 

sideslip measurement, 126-27 


Zero lateral velocity (ZLV), 82-83 


